text:
import csv
import glob
import os
import random
import sys

import time
from math import fabs
from pathlib import Path

import numpy as np
import torch
import torch.nn.functional as F
from tqdm import tqdm

from model import SpeakerNet
from utils import tuneThresholdfromScore

def inference(args):
    model = SpeakerNet(**vars(args))
    model_save_path = args.save_path + f"/{args.model}/model"
    result_save_path = args.save_path + f"/{args.model}/result"
    # Write args to score_file
    settings_file = open(result_save_path + '/settings.txt', 'a+')
    score_file = open(result_save_path + "/scores.txt", "a+")
    # summary settings
    settings_file.write(
        f'\n[INFER]------------------{time.strftime("%Y-%m-%d %H:%M:%S")}------------------\n')
    score_file.write(
        f'\n[INFER]------------------{time.strftime("%Y-%m-%d %H:%M:%S")}------------------\n')
    # write the settings to settings file
    for items in vars(args):
        # print(items, vars(args)[items])
        settings_file.write('%s %s\n' % (items, vars(args)[items]))
    settings_file.flush()
    settings_file.close()

    # priority: define weight -> best weight -> last weight
    if args.initial_model_infer:
        chosen_model_state = args.initial_model_infer
    elif os.path.exists(f'{model_save_path}/best_state.model'):
        chosen_model_state = f'{model_save_path}/best_state.model'
    else:
        model_files = glob.glob(os.path.join(
            model_save_path, 'model_state_*.model'))
        chosen_model_state = model_files[-1]

    print(f'Loading model from {chosen_model_state}')
    model.loadParameters(chosen_model_state)

    model.eval()
    # set defalut threshold
    threshold = args.test_threshold
    scoring_mode = args.scoring_mode
    num_eval = 10

    # Evaluation code
    if args.eval is True:
        sc, lab, trials = model.evaluateFromList(
            args.test_list,
            cohorts_path=args.cohorts_path,
            print_interval=10,
            eval_frames=args.eval_frames,
            scoring_mode=scoring_mode)
        target_fa = np.linspace(5, 0, num=50)
        result = tuneThresholdfromScore(sc, lab, target_fa)
        print('tfa [thre, fpr, fnr]')
        best_sum_rate = 999
        best_tfa = None
        for i, tfa in enumerate(target_fa):
#             print(tfa, result[0][i])
            sum_rate = result[0][i][1] + result[0][i][2]
            if sum_rate < best_sum_rate:
                best_sum_rate = sum_rate
                best_tfa = result[0][i]
        print(f'Best sum rate {best_sum_rate} at {best_tfa}')
        print(f'EER {result[1]}% at threshold {result[2]}')
        print(f'AUC {result[3]}')
        score_file.write(
            f"Evaluation result on:\n\
            Best sum rate {best_sum_rate} at {best_tfa}\n\
            EER {result[1]} at threshold {result[2]}\nAUC {result[3]}")
        score_file.close()

        # write to file
        save_root = args.save_path + f"/{args.model}/result"
        write_file = Path(save_root, 'evaluation_results.txt')

        with open(write_file, 'w', newline='') as wf:
            spamwriter = csv.writer(wf, delimiter=',')
            spamwriter.writerow(['audio_1', 'audio_2','label', 'score', 'predict_label'])
            for score, label, pair in zip(sc, lab, trials):
                pred = '1' if score >= result[2] else '0'
                com, ref = pair.strip().split(' ')
                spamwriter.writerow([com, ref, label, score, pred])
            print("Done")

        sys.exit(1)

    # Test code
    if args.test is True:
        model.testFromList(args.test_path,
                           cohorts_path=args.cohorts_path,
                           thre_score=threshold,
                           print_interval=1,
                           eval_frames=args.eval_frames,
                           scoring_mode=scoring_mode)
        sys.exit(1)

    if args.test_by_pair is True:
        model.test_each_pair(args.test_path,
                             cohorts_path=args.cohorts_path,
                             thre_score=threshold,
                             print_interval=1,
                             eval_frames=args.eval_frames,
                             scoring_mode=scoring_mode)
        sys.exit(1)

    # Prepare embeddings for cohorts/verification
    if args.prepare is True:
        model.prepare(eval_frames=args.eval_frames,
                      from_path=args.test_list,
                      save_path=args.cohorts_path,
                      num_eval=num_eval,
                      prepare_type=args.prepare_type)
        sys.exit(1)

    # Predict
    if args.predict is True:
        """
        Predict new utterance based on distance between its embedding and saved embeddings.
        """
        embeds_path = Path(args.save_path, 'embeds.pt')
        classes_path = Path(args.save_path, 'classes.npy')
        embeds = torch.load(embeds_path).to(torch.device(args.device))
        classes = np.load(str(classes_path), allow_pickle=True).item()
        if args.test_list.endswith('.txt'):
            files = []
            with open(args.test_list) as listfile:
                while True:
                    line = listfile.readline()
                    if not line:
                        break
                    data = line.split()

                    # Append random label if missing TODO: ? why random
                    if len(data) == 2:
                        data = [random.randint(0, 1)] + data

                    files.append(Path(data[1]))
                    files.append(Path(data[2]))

            files = list(set(files))
        else:
            files = list(Path(args.test_list).glob('*/*.wav'))
        files.sort()

        same_smallest_score = 1
        diff_biggest_score = 0
        for f in tqdm(files):
            embed = model.embed_utterance(f,
                                          eval_frames=args.eval_frames,
                                          num_eval=num_eval,
                                          normalize=model.__L__.test_normalize)
            embed = embed.unsqueeze(-1)
            dist = F.pairwise_distance(embed, embeds).detach().cpu().numpy()
            dist = np.mean(dist, axis=0)
            score = 1 - np.min(dist) ** 2 / 2
            if classes[np.argmin(dist)] == f.parent.stem:
                if score < same_smallest_score:
                    same_smallest_score = score
                indexes = np.argsort(dist)[:2]
                if fabs((1 - dist[indexes[0]] ** 2 / 2) - (1 - dist[indexes[1]] ** 2 / 2)) < 0.001:
                    for i, idx in enumerate(indexes):
                        score = 1 - dist[idx] ** 2 / 2
                        if i == 0:
                            tqdm.write(f'+ {f}, {score} - {classes[idx]}',
                                       end='; ')
                        else:
                            tqdm.write(f'{score} - {classes[idx]}', end='; ')
                    tqdm.write('***')
                else:
                    tqdm.write(f'+ {f}, {score}', end='')
                    if score < args.test_threshold:
                        tqdm.write(' ***', end='')
                    tqdm.write('')
            else:
                if score > diff_biggest_score:
                    diff_biggest_score = score
                if score > args.test_threshold:
                    indexes = np.argsort(dist)[:3]
                    # TODO: duplicated from line 112 - 119
                    for i, idx in enumerate(indexes):
                        score = 1 - dist[idx] ** 2 / 2
                        if i == 0:
                            tqdm.write(f'- {f}, {score} - {classes[idx]}',
                                       end='; ')
                        else:
                            tqdm.write(f'{score} - {classes[idx]}', end='; ')
                    tqdm.write('***')
        print(f'same_smallest_score: {same_smallest_score}')
        print(f'diff_biggest_score: {diff_biggest_score}')
        sys.exit(1)

max_stars_repo_name: hiimmuc/Speaker-verification

max_issues_repo_name: hiimmuc/Speaker-verification

max_forks_repo_name: hiimmuc/Speaker-verification


text:
"""Test bokeh server for debugging / feature development."""
import click
import numpy as np

from bokeh.server.server import Server
from bokeh.application import Application
from bokeh.application.handlers.function import FunctionHandler
from bokeh.plotting import figure
from bokeh.layouts import layout
from bokeh.models.widgets import Button

def image_array():
    N = 500
    x = np.linspace(0, 10, N)
    y = np.linspace(0, 10, N)
    xx, yy = np.meshgrid(x, y)
    img = np.sin(xx)*np.cos(yy)
    return img

def make_makedoc():
    """Make the makedoc function required by Bokeh Server."""

    def makedoc(doc):
        img = image_array()
        p = figure(tooltips=[("x", "$x"), ("y", "$y"), ("value", "@image")])
        p.x_range.range_padding = p.y_range.range_padding = 0
        # must give a vector of image data for image parameter
        p.image(image=[img], x=0, y=0, dw=10, dh=10, palette="Spectral11")
        controls = [Button(label="button 1", button_type="success"),
                    Button(label="button 1", button_type="success")]

        page_content = layout([
            [p],
            controls,
            ], sizing_mode="scale_width")
        doc.title = 'Test bokeh app'
        doc.add_root(page_content)
    print('ready!')
    return makedoc

@click.command()
@click.option('-c', '--config', default=None)
@click.option('-p', '--path', default='/')
@click.option('-P', '--port', type=int, default=5000)
def run_server_cmd(config=None, path='/', port=5000):
    run_server(config=config, path=path, port=port)

def run_server(config=None, path='/', port=5000):
    """Run the bokeh server."""
    makedoc = make_makedoc()
    apps = {path: Application(FunctionHandler(makedoc))}

    server = Server(apps, port=port, allow_websocket_origin=['*'])
    server.run_until_shutdown()

if __name__ == '__main__':
    run_server_cmd()

ext: py

max_stars_repo_name: microscopium/test-bokeh-server

max_issues_repo_name: microscopium/test-bokeh-server

max_forks_repo_name: microscopium/test-bokeh-server

reason: import numpy


text:
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

import sys
import numpy as np
import gc
from .VAE import *
from .AE import *
from .utils import *

class Splitter(object):
	def __init__(self, X_train, reducer, clustering, z_dim=2, network_architecture=None):

		self.X_train,self.xmin,self.xmax = standard(X_train)
		self.n_samples, self.n_ftrs = X_train.shape
		self.z_dim = z_dim
		if reducer=='none':
			self.z_dim = self.n_ftrs
		if network_architecture is None:
			network_architecture = [[self.n_ftrs,self.n_ftrs//2,self.z_dim],[self.z_dim,self.n_ftrs//2,self.n_ftrs]]
		self.network_architecture = network_architecture
		self.reducer = reducer
		self.t_ind = np.arange(self.n_samples)
		self.clustering = clustering
		self.step = 0
		self.X = []
		self.z_mu = []
		self.labels = []
		self.mean_points = []
		self.mean_components = []
		self.dense_points = []
		self.dense_components = []
		self.mean_vector = []
		self.covariance = []
#		self.models = []
		self.true_indices = []

	def __del__(self):
		del self.X_train
		del self.n_samples
		del self.n_ftrs
		del self.t_ind
		del self.clustering
		del self.step
		del self.X
		del self.z_mu
		del self.labels
		del self.mean_points
		del self.mean_components
		del self.dense_points
		del self.dense_components
		del self.mean_vector
		del self.covariance
		del self.true_indices

	def call_model(self,reducer,network_architecture):
# Autoencoder
		if reducer=='AE':
			model = AutoEncoder(network_architecture)

# Variational Autoencoder
		elif reducer=='VAE':
			model = VariationalAutoEncoder(network_architecture)

# Without dimensionality reduction
		elif reducer=='none':
			model = n_model()

		else:
			try:
				model = sk_convert(reducer)
			except:
				assert 0,'Unknown transformation!\n \
									You can used Scikit-learn dimensionality reduction methods like PCA, ICA or NMF.\n \
									Or if you want to use a custom dimensionality reduction methods, It should be a \n \
									class and has fit, transform and inverse_transform methods.'

		return model

	def reset_step(self):
		self.step = 0

	def _process(self, X_in,clustering,
							labels=None,
							ind=None,
							training_epochs = 20,
							verbose=True,
							save=None):

		if ind is None:
			X = X_in
		else:
			X = X_in[labels==ind]

		xmin = self.xmin
		xmax = self.xmax

		mean_points = np.zeros((2,self.z_dim))
		mean_components = np.zeros((2, self.n_ftrs))
		dense_points = np.zeros((2,self.z_dim))
		dense_components = np.zeros((2, self.n_ftrs))
		mean_vector = np.zeros((2, self.n_ftrs))
		covariance = np.array([np.eye(self.n_ftrs),np.eye(self.n_ftrs)])

		if X.shape[0]==0:
			return np.array([]),np.array([]),np.array([]),mean_points,mean_components,dense_points,dense_components,mean_vector,covariance

		if X.shape[0]==1:
			z_mu = np.array([[0.5,0.5]])
			for j in [0]:
				mean_components[j,:] = X
				if self.z_dim==2:
					dense_components[j,:] = X
				mean_vector[j,:] = X
			return X,z_mu*xmax+xmin,np.array([0]),z_mu*xmax+xmin,mean_components*xmax+xmin,\
							z_mu*xmax+xmin,dense_components*xmax+xmin,mean_vector,covariance

#########
		with self.call_model(self.reducer,self.network_architecture) as model:

			model.train(X, training_epochs=training_epochs, verbose=verbose)
			z_mu = model.encoder(X)

	#		label_out = self.clustering(z_mu)
			label_out = op_cluster(self.clustering, z_mu)

			for j in [0,1]:
				pts = z_mu[label_out==j]
				if pts.shape[0]!=0:
					mean_points[j,:] = np.mean(pts,axis=0)
					mean_components[j,:] = model.decoder((mean_points[j,:]).reshape(1,self.z_dim))

					if self.z_dim==2:
						dense_points[j,:] = dense_point(pts)
						dense_components[j,:] = model.decoder((dense_points[j,:]).reshape(1,self.z_dim))
	#############################################

			for j in [0,1]:
				X_sub = X[label_out==j]*xmax+xmin
				if X_sub.shape[0]>1:
					mean_vector[j,:] = np.mean(X_sub,axis=0)
					covariance[j,:] = Cov_mat(X_sub)

				elif X_sub.shape[0]==1:
					mean_vector[j,:] = X_sub[0]

			if save is not None:
				try:
					model.save(save)
				except:
					print ('Dimensionality reduction method has no save attribute!')

		gc.collect()

		return X,z_mu*xmax+xmin,label_out,mean_points*xmax+xmin,mean_components*xmax+xmin,\
						dense_points*xmax+xmin,dense_components*xmax+xmin,mean_vector,covariance #,model

	def split(self, n_s,
						clustering=None,
						training_epochs=20,
						verbose=True,
						save=None):

		if clustering is None:
			clustering = self.clustering

		k_max = n_s
		if self.step==0:

			self.step += 1
			if verbose:
				print ('Split level:',self.step)

			if save is not None:
				name = 'level'+str(self.step)
				if save[-1]=='/':
					save_name = save+name+'_model.ckpt'
				else:
					save_name = save+'/'+name+'_model.ckpt'
			else:
				save_name = None

			X,z_mu,labels_out,mean_points,mean_components,dense_points,dense_components,mean_vector,covariance = \
								self._process(self.X_train,clustering,
														 training_epochs=training_epochs,
														 verbose=verbose, save=save_name)

			self.X = [X]
			self.z_mu = [z_mu]
			self.lbls = [labels_out]
			self.labels = [label_maker(self.lbls)]
			self.mean_points = [mean_points.reshape(1,2,self.z_dim)]
			self.mean_components = [mean_components.reshape(1,2,self.n_ftrs)]
			self.dense_points = [dense_points.reshape(1,2,self.z_dim)]
			self.dense_components = [dense_components.reshape(1,2,self.n_ftrs)]
			self.mean_vector = [mean_vector.reshape(1,2,self.n_ftrs)]
			self.covariance = [covariance.reshape(1,2,self.n_ftrs,self.n_ftrs)]
#			self.models = [model]
			self.true_indices = [self.t_ind[labels_out==0],self.t_ind[labels_out==1]]

			ngc = gc.collect()
			k_max -= 1

		if self.step!=0:
			k = 0
			while True:
				k += 1
				if (k>k_max):
					break
				self.step += 1
				if verbose:
					print ('--------------------------')
					print ('Split level: ',self.step)

				X0 = []
				z_mu0 = []
				lbls0 = []
				mean_points0 = []
				mean_components0 = []
				dense_points0 = []
				dense_components0 = []
				mean_vector0 = []
				covariance0 = []
#				models0 = []
				t_ind0 = []

				for j in range(len(self.X)):
					for ind in range(2):

						if save is not None:
							name = 'level'+str(self.step)+'_c'+str(2*j+ind)
							if save[-1]=='/':
								save_name = save+name+'_model.ckpt'
							else:
								save_name = save+'/'+name+'_model.ckpt'
						else:
							save_name = None

						Xp,z_mu,labels_out,mean_points,mean_components,dense_points,dense_components,mean_vector,covariance = \
																	self._process(
																	self.X[j],
																	clustering,
																	labels = self.lbls[j],
																	ind = ind,
																	training_epochs=training_epochs,
																	verbose = verbose,
																	save=save_name)

						X0.append(Xp)
						z_mu0.append(z_mu)
						lbls0.append(labels_out)
						mean_points0.append(mean_points)
						mean_components0.append(mean_components)
						dense_points0.append(dense_points)
						dense_components0.append(dense_components)
						mean_vector0.append(mean_vector)
						covariance0.append(covariance)
#						models0.append(model)
						t_ind0.append(self.true_indices[2*j+ind][labels_out==0])
						t_ind0.append(self.true_indices[2*j+ind][labels_out==1])

				self.X = X0
				self.z_mu = z_mu0
				self.lbls = lbls0
				self.labels.append(label_maker(self.lbls))
				self.mean_points.append(np.array(mean_points0))
				self.mean_components.append(np.array(mean_components0))
				self.dense_points.append(np.array(dense_points0))
				self.dense_components.append(np.array(dense_components0))
				self.mean_vector.append(np.array(mean_vector0))
				self.covariance.append(np.array(covariance0))
#				self.models = models0
				self.true_indices = t_ind0

				ngc = gc.collect()

#	def mean_components(self):
#		points = np.zeros((2**(self.step),2))
#		components = np.zeros((2**(self.step), self.n_ftrs))
#		for i,z in enumerate(self.z_mu):
#			for j in [0,1]:
#				pts = z[self.lbls[i]==j]
#				if pts.shape[0]!=0:
#					points[2*i+j,:] = np.mean(pts,axis=0)
#					components[2*i+j,:] = self.models[i].decoder((points[2*i+j,:]).reshape(1,2))
#				else:
#					points[2*i+j,:] = np.array([0,0])
#					components[2*i+j,:] = np.zeros(self.n_ftrs)
#		return points,components

#	def dense_components(self):
#		points = np.zeros((2**(self.step),2))
#		components = np.zeros((2**(self.step), self.n_ftrs))
#		for i,z in enumerate(self.z_mu):
#			for j in [0,1]:
#				pts = z[self.lbls[i]==j]
#				if pts.shape[0]!=0:
#					points[2*i+j,:] = np.mean(pts,axis=0)
#					components[2*i+j,:] = self.models[i].decoder((points[2*i+j,:]).reshape(1,2))
#				else:
#					points[2*i+j,:] = np.array([0,0])
#					components[2*i+j,:] = np.zeros(self.n_ftrs)
#		return points,components

def label_maker(labels):
	lbls = np.array([])
	for i,lst in enumerate(labels):
		lbls = np.append(lbls,lst+2*i)
	return lbls

class n_model(object):
	def __init__(self):
		pass
	def train(self,X, training_epochs=None, verbose=None):
		pass
	def encoder(self,X):
		  return X
	def decoder(self,z):
		  return z
	def __enter__(self):
		  return self
	def __exit__(self, type, value, traceback):
		  pass

max_stars_repo_name: vafaei-ar/pymce

max_issues_repo_name: vafaei-ar/pymce

max_forks_repo_name: vafaei-ar/pymce


text:
"""
  SubT Challenge Version 1
"""
import gc
import os.path
import math
import threading
import copy

from datetime import timedelta
from collections import defaultdict
from io import StringIO

import numpy as np

from osgar.explore import follow_wall_angle
from osgar.lib.mathex import normalizeAnglePIPI
from osgar.lib import quaternion
from osgar.lib.virtual_bumper import VirtualBumper
from osgar.lib.lidar_pts import equal_scans
from osgar.lib.loop import LoopDetector

from subt.local_planner import LocalPlanner
from subt.trace import Trace, distance3D
from subt.name_decoder import parse_robot_name

# safety limits for exploration and return home
DEFAULT_LIMIT_ROLL = 28  # degrees
DEFAULT_LIMIT_PITCH = 28  # degrees
DEFAULT_RETURN_LIMIT_ROLL = 35  # degrees
DEFAULT_RETURN_LIMIT_PITCH = 35  # degrees

# accepted LoRa commands
LORA_GO_HOME_CMD = b'GoHome'
LORA_STOP_CMD = b'Stop'
LORA_PAUSE_CMD = b'Pause'
LORA_CONTINUE_CMD = b'Continue'

# reasons for termination of follow wall
REASON_DIST_REACHED = 'dist_reached'
REASON_PITCH_LIMIT = 'pitch_limit'
REASON_ROLL_LIMIT = 'roll_limit'
REASON_VIRTUAL_BUMPER = 'virtual_bumper'
REASON_LORA = 'lora'
REASON_FRONT_BUMPER = 'front_bumper'
REASON_REAR_BUMPER = 'rear_bumper'
REASON_LOOP = 'loop'

def any_is_none(*args):
    for a in args:
        if a is None:
            return True
    return False

def distance(pose1, pose2):
    return math.hypot(pose1[0] - pose2[0], pose1[1] - pose2[1])

class Collision(Exception):
    pass

class EmergencyStopException(Exception):
    pass

class NewWaypointsException(Exception):
    pass

class NotMovingException(Exception):
    pass

class HomeReachedException(Exception):
    pass

class EmergencyStopMonitor:
    def __init__(self, robot):
        self.robot = robot

    def update(self, robot):
        if robot.emergency_stop:
            raise EmergencyStopException()

        # handle STOP independently of current subroutine
        if robot.lora_cmd == LORA_STOP_CMD:
            print(robot.time, 'LoRa cmd - Stop')
            raise EmergencyStopException()

    # context manager functions
    def __enter__(self):
        self.callback = self.robot.register(self.update)
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.robot.unregister(self.callback)

class NewWaypointsMonitor:
    def __init__(self, robot):
        self.robot = robot

    def update(self, robot):
        if robot.waypoints is not None:
            raise NewWaypointsException()

    # context manager functions
    def __enter__(self):
        self.callback = self.robot.register(self.update)
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.robot.unregister(self.callback)

class NotMovingMonitor:
    """
    Check that for given time robot leaves sphere of given radius
    """
    def __init__(self, robot, radius, sim_time_sec_period):
        self.robot = robot
        self.radius = radius
        self.sim_time_sec_period = sim_time_sec_period
        self.anchor_xyz = None
        self.anchor_sim_time = None

    def update(self, robot):
        if robot.xyz is not None and robot.sim_time_sec is not None:
            if self.anchor_xyz is None:
                # the very first anchor
                self.anchor_xyz = robot.xyz
                self.anchor_sim_time = robot.sim_time_sec
            else:
                if distance3D(robot.xyz, self.anchor_xyz) > self.radius:
                    self.anchor_xyz = robot.xyz
                    self.anchor_sim_time = robot.sim_time_sec
                if robot.sim_time_sec - self.anchor_sim_time > self.sim_time_sec_period:
                    raise NotMovingException()

    # context manager functions
    def __enter__(self):
        self.callback = self.robot.register(self.update)
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.robot.unregister(self.callback)

class HomeReachedMonitor:
    """
    Check it the robot got close to the (0, 0, 0) point.
    """
    def __init__(self, robot, radius):
        self.robot = robot
        self.radius = radius

    def update(self, robot):
        if distance3D(robot.xyz, (0, 0, 0)) < self.radius:
            raise HomeReachedException()

    # context manager functions
    def __enter__(self):
        self.callback = self.robot.register(self.update)
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.robot.unregister(self.callback)

class SubTChallenge:
    def __init__(self, config, bus):
        self.bus = bus
        bus.register("desired_speed", "pose2d", "stdout", "desired_z_speed", "flipped")
        self.traveled_dist = 0.0
        self.time = None
        self.max_speed = config['max_speed']
        self.max_angular_speed = math.radians(60)
        self.rotation_p = config.get('rotation_p', 0.8)
        self.turbo_speed = config.get('turbo_speed')
        self.cautious_speed = config.get('cautious_speed')
        self.min_speed = config.get('min_speed', 0.05)
        self.gap_size = config['gap_size']
        self.robot_width = config.get('robot_width', self.gap_size)
        self.wall_dist = config['wall_dist']
        self.follow_wall_params = config.get('follow_wall', {})
        self.timeout = timedelta(seconds=config['timeout'])
        self.symmetric = config['symmetric']  # is robot symmetric?
        self.dangerous_dist = config.get('dangerous_dist', 0.3)
        self.min_safe_dist = config.get('min_safe_dist', 0.75)
        self.turbo_safe_dist = config.get('turbo_safe_dist')
        self.safety_turning_coeff = config.get('safety_turning_coeff', 0.8)
        self.limit_pitch = math.radians(config.get('limit_pitch', DEFAULT_LIMIT_PITCH))
        self.limit_roll = math.radians(config.get('limit_roll', DEFAULT_LIMIT_ROLL))
        self.return_limit_pitch = math.radians(config.get('return_limit_pitch', DEFAULT_RETURN_LIMIT_PITCH))
        self.return_limit_roll = math.radians(config.get('return_limit_roll', DEFAULT_RETURN_LIMIT_ROLL))
        virtual_bumper_sec = config.get('virtual_bumper_sec')
        self.virtual_bumper = None
        if virtual_bumper_sec is not None:
            virtual_bumper_radius = config.get('virtual_bumper_radius', 0.1)
            self.virtual_bumper = VirtualBumper(timedelta(seconds=virtual_bumper_sec), virtual_bumper_radius)
        self.speed_policy = config.get('speed_policy', 'always_forward')
        assert(self.speed_policy in ['always_forward', 'conservative'])
        self.height_above_ground = config.get('height_above_ground', 0.0)
        self.trace_z_weight = config.get('trace_z_weight', 0.2)  # Z is important for drones ~ 3.0
        self.neighborhood_size = config.get('neighborhood_size', 12.0)
        self.approach_angle = math.radians(config.get('approach_angle', 45))

        self.gc_interval = config.get('gc_interval', 100)
        self.gc_n = 0

        self.last_position = (0, 0, 0)  # proper should be None, but we really start from zero
        self.xyz = None  # unknown initial 3D position
        self.yaw, self.pitch, self.roll = 0, 0, 0
        self.orientation = None  # quaternion updated by on_pose3d()
        self.yaw_offset = None  # not defined, use first IMU reading
        self.is_moving = None  # unknown
        self.scan = None  # I should use class Node instead
        self.slopes = None
        self.flipped = False  # by default use only front part
        self.flipping_pose = self.last_position  # Last position where the robot flipped.
        self.joint_angle_rad = []  # optinal angles, needed for articulated robots flip
        self.stat = defaultdict(int)
        self.voltage = []
        self.whereabouts = {}  # Whereabouts of other robots.
        self.trace = Trace()
        self.waypoints = None  # external waypoints for navigation
        self.loop_detector = LoopDetector()
        self.collision_detector_enabled = False
        self.sim_time_sec = 0

        self.lora_cmd = None

        self.emergency_stop = None
        self.monitors = []  # for Emergency Stop Exception

        self.use_right_wall = config['right_wall']
        self.use_center = False  # navigate into center area (controlled by name ending by 'C')
        self.is_virtual = config.get('virtual_world', False)  # workaround to handle tunnel differences

        self.front_bumper = False
        self.rear_bumper = False

        self.last_send_time = None

        self.init_path = None
        if 'init_path' in config:
            pts_s = [s.split(',') for s in config['init_path'].split(';')]
            self.init_path = [(float(x), float(y)) for x, y in pts_s]
        self.robot_name = config.get('robot_name')
        scan_subsample = config.get('scan_subsample', 1)
        obstacle_influence = config.get('obstacle_influence', 0.8)
        direction_adherence = math.radians(config.get('direction_adherence', 90))
        max_obstacle_distance = config.get('max_obstacle_distance', 2.5)
        self.local_planner = LocalPlanner(
                obstacle_influence=obstacle_influence,
                direction_adherence=direction_adherence,
                max_obstacle_distance=max_obstacle_distance,
                scan_subsample=scan_subsample,
                max_considered_obstacles=100)
        self.use_return_trace = config.get('use_return_trace', True)
        self.ref_scan = None
        self.pause_start_time = None
        if config.get('start_paused', False):
            self.pause_start_time = timedelta()  # paused from the very beginning

        self.scan_directions = None
        self.scan_directions_cos = None
        self.scan_directions_sin = None

    def is_home(self):
        HOME_RADIUS = 20.0
        home_position = self.trace.start_position()
        return home_position is None or distance3D(self.last_position, home_position) < HOME_RADIUS

    def is_approaching_another_robot(self):
        if self.neighborhood_size is None or self.approach_angle is None:
            return False

        yaw = normalizeAnglePIPI(self.yaw + math.pi) if self.flipped else self.yaw
        for other_xyz in self.whereabouts.values():
            d = distance3D(self.xyz, other_xyz)
            angle = normalizeAnglePIPI(math.atan2(other_xyz[1] - self.xyz[1], other_xyz[0] - self.xyz[0]) - yaw)
            if d <= self.neighborhood_size and abs(angle) < self.approach_angle:
                return True

        return False

    def speed_limit(self):
        if self.scan_directions is None or self.scan_directions.shape[0] != len(self.scan):
            self.scan_directions = np.radians(np.linspace(-135, 135, num=len(self.scan)))
            self.scan_directions_cos = np.cos(self.scan_directions)
            self.scan_directions_sin = np.sin(self.scan_directions)
        scan = np.asarray(self.scan) / 1000.0
        pts_x = scan * self.scan_directions_cos
        pts_y = scan * self.scan_directions_sin
        of_interest = np.logical_and.reduce(np.stack([
                scan > 0.01,
                pts_x > 0,
                np.abs(pts_y) <= self.robot_width / 2]))
        pts_x = pts_x[of_interest]
        pts_y = pts_y[of_interest]
        dists = np.hypot(pts_x, pts_y)
        dist = np.inf if dists.size == 0 else float(np.min(dists))
        if dist < self.min_safe_dist:
            safe_speed = self.max_speed * (dist - self.dangerous_dist) / (self.min_safe_dist - self.dangerous_dist)
            desired_speed = safe_speed if self.cautious_speed is None or not self.is_approaching_another_robot() else min(safe_speed, self.cautious_speed)
            if 0 <= desired_speed < self.min_speed:
                desired_speed = self.min_speed
            elif -self.min_speed < desired_speed < 0:
                desired_speed = -self.min_speed
            return desired_speed
        elif self.cautious_speed is not None and self.is_approaching_another_robot():
            return self.cautious_speed
        elif self.turbo_safe_dist is not None and self.turbo_speed is not None and dist > self.turbo_safe_dist and not self.is_home():
            return self.turbo_speed
        return self.max_speed

    def send_speed_cmd(self, speed, angular_speed):
        if self.virtual_bumper is not None:
            self.virtual_bumper.update_desired_speed(speed, angular_speed)
        self.bus.publish('desired_speed', [round(speed*1000), round(math.degrees(angular_speed)*100)])
        self.gc_n += 1
        if self.gc_n == self.gc_interval:
            # Corresponds to gc.disable() in main(). See a comment there for more details.
            gc.collect()
            self.gc_n = 0

    def go_straight(self, how_far, timeout=None):
        print(self.time, "go_straight %.1f (speed: %.1f)" % (how_far, self.max_speed), self.last_position, self.flipped)
        start_pose = self.last_position
        if self.flipped:
            how_far = -how_far  # flip desired direction to current robot front
        if how_far >= 0:
            self.send_speed_cmd(self.max_speed, 0.0)
        else:
            self.send_speed_cmd(-self.max_speed, 0.0)
        start_time = self.time
        while distance(start_pose, self.last_position) < abs(how_far):
            if self.slopes:
                slope_idx = 0 if how_far < 0 else len(self.slopes) // 2
                slope = self.slopes[slope_idx]
                desired_z_speed = float(self.max_speed * math.tan(slope))
                self.bus.publish('desired_z_speed', desired_z_speed)
            self.update()
            if timeout is not None and self.time - start_time > timeout:
                print("go_straight - TIMEOUT!")
                break
        self.send_speed_cmd(0.0, 0.0)

    def go_safely(self, desired_direction, allow_z_control=True):
        if self.local_planner is None:
            safety, safe_direction = 1.0, desired_direction
        else:
            safety, safe_direction = self.local_planner.recommend(desired_direction)
        if self.flipped and self.joint_angle_rad:
            safe_direction = normalizeAnglePIPI(safe_direction + sum(self.joint_angle_rad))
        #print(self.time,"safety:%f    desired:%f  safe_direction:%f"%(safety, desired_direction, safe_direction))
        desired_angular_speed = self.rotation_p * safe_direction
        desired_speed = self.speed_limit() * (1.0 - self.safety_turning_coeff * min(self.max_angular_speed, abs(desired_angular_speed)) / self.max_angular_speed)
        if allow_z_control and self.slopes:
            slope_idx = int(len(self.slopes) * (safe_direction - -math.pi) / (2 * math.pi))
            slope = self.slopes[slope_idx]
            desired_z_speed = float(abs(desired_speed) * math.tan(slope))
            self.bus.publish('desired_z_speed', desired_z_speed)
        if self.flipped:
            self.send_speed_cmd(-desired_speed, desired_angular_speed)
        else:
            self.send_speed_cmd(desired_speed, desired_angular_speed)
        return safety, safe_direction

    def turn(self, angle, with_stop=True, speed=0.0, timeout=None):
        print(self.time, "turn %.1f" % math.degrees(angle))

        if self.slopes:
            self.bus.publish('desired_z_speed', 0)

        start_pose = self.last_position
        if angle >= 0:
            self.send_speed_cmd(speed, self.max_angular_speed)
        else:
            self.send_speed_cmd(speed, -self.max_angular_speed)
        start_time = self.time
        while abs(normalizeAnglePIPI(start_pose[2] - self.last_position[2])) < abs(angle):
            self.update()
            if timeout is not None and self.time - start_time > timeout:
                print(self.time, "turn - TIMEOUT!")
                break
        if with_stop:
            self.stop(2)

    def stop(self, timeout_sec=20):
        self.send_speed_cmd(0.0, 0.0)

        if self.slopes:
            self.bus.publish('desired_z_speed', 0)

        start_time = self.time
        while self.time - start_time < timedelta(seconds=timeout_sec):
            self.update()
            if not self.is_moving:
                break
        print(self.time, 'stop at', self.time - start_time, self.is_moving)

    def can_flip(self):
        return self.symmetric and distance3D(self.last_position, self.flipping_pose) > 1.0

    def flip(self, with_stop=True):
        # make sure that we will use clean data
        self.scan = None

        self.flipped = not self.flipped
        self.flipping_pose = self.last_position
        self.bus.publish('flipped', self.flipped)

        if with_stop:
            self.stop(2)

        # Making sure we have scan data. Parts of the code assume that.
        while self.scan is None:
            self.update()

    def follow_wall(self, gap_size, wall_dist, right_wall=False, timeout=timedelta(hours=3), dist_limit=None,
            pitch_limit=None, roll_limit=None, detect_loop=True):
        reason = None  # termination reason is not defined yet
        start_dist = self.traveled_dist
        start_time = self.sim_time_sec

        last_pause_time = timedelta()  # for multiple Pause
        current_pause_time = timedelta()
        while self.sim_time_sec - start_time < (timeout + last_pause_time + current_pause_time).total_seconds():
            try:
                channel = self.update()
                if (channel == 'scan' and not self.flipped) or (channel == 'scan_back' and self.flipped) or channel == 'scan360':
                    if self.pause_start_time is None:
                        if self.use_center:
                            self.go_safely(0)
                        else:
                            desired_direction = normalizeAnglePIPI(
                                    follow_wall_angle(self.scan, gap_size=gap_size, wall_dist=wall_dist, right_wall=right_wall, **self.follow_wall_params))
                            flip_threshold = math.radians(130)  # including some margin around corners
                            if self.can_flip() and (
                                    (right_wall and desired_direction > flip_threshold) or
                                    (not right_wall and desired_direction < -flip_threshold)):
                                print('Flipping:', math.degrees(desired_direction))
                                self.flip()
                            else:
                                self.go_safely(desired_direction)
                if dist_limit is not None:
                    if dist_limit < abs(self.traveled_dist - start_dist):  # robot can return backward -> abs()
                        print(self.time, 'Distance limit reached! At', self.traveled_dist, self.traveled_dist - start_dist)
                        reason = REASON_DIST_REACHED
                        break
                if pitch_limit is not None and self.pitch is not None:
                    if abs(self.pitch) > pitch_limit:
                        print(self.time, 'Pitch limit triggered termination: (pitch %.1f)' % math.degrees(self.pitch))
                        reason = REASON_PITCH_LIMIT
                        break
                if roll_limit is not None and self.roll is not None:
                    if abs(self.roll) > roll_limit:
                        print(self.time, 'Roll limit triggered termination: (roll %.1f)' % math.degrees(self.roll))
                        reason = REASON_ROLL_LIMIT
                        break
                if self.virtual_bumper is not None and self.virtual_bumper.collision():
                    print(self.time, "VIRTUAL BUMPER - collision")
                    self.go_straight(-0.3, timeout=timedelta(seconds=10))
                    reason = REASON_VIRTUAL_BUMPER
                    break

                loop = detect_loop and self.loop_detector.loop()
                if loop:
                    print(self.time, self.sim_time_sec, 'Loop detected')
                    turn_timeout = timedelta(seconds=40)
                    if self.symmetric:
                        self.flip()
                        if self.speed_policy == 'conservative':
                            self.turn(math.radians(-20 if self.use_right_wall else 20), timeout=turn_timeout, with_stop=True)
                    else:
                        self.turn(math.radians(160 if self.use_right_wall else -160), timeout=turn_timeout, with_stop=True)
                    reason = REASON_LOOP
                    break

                if self.front_bumper and not self.flipped:
                    print(self.time, "FRONT BUMPER - collision")
                    self.go_straight(-0.3, timeout=timedelta(seconds=10))
                    reason = REASON_FRONT_BUMPER
                    break

                if self.rear_bumper and self.flipped:
                    print(self.time, "REAR BUMPER - collision")
                    self.go_straight(-0.3, timeout=timedelta(seconds=10))
                    reason = REASON_REAR_BUMPER
                    break

                if self.lora_cmd is not None:
                    # the "GoHome" command must be accepted only on the way there and not on the return home
                    if dist_limit is None and self.lora_cmd == LORA_GO_HOME_CMD:
                        print(self.time, 'LoRa cmd - GoHome')
                        self.lora_cmd = None
                        reason = REASON_LORA
                        break
                    if self.lora_cmd == LORA_PAUSE_CMD:
                        print(self.time, 'LoRa cmd - Pause')
                        self.send_speed_cmd(0, 0)
                        if self.pause_start_time is None:
                            # ignore repeated Pause
                            self.pause_start_time = self.time
                        self.lora_cmd = None
                    elif self.lora_cmd == LORA_CONTINUE_CMD:
                        print(self.time, 'LoRa cmd - Continue')
                        if self.pause_start_time is not None:
                            # ignore Continue without Pause
                            last_pause_time += self.time - self.pause_start_time
                            self.pause_start_time = None
                        self.lora_cmd = None
                if self.pause_start_time is not None:
                    current_pause_time = self.time - self.pause_start_time
                else:
                    current_pause_time = timedelta()
            except Collision:
                assert not self.collision_detector_enabled  # collision disables further notification
                before_stop = self.xyz
                self.stop()
                after_stop = self.xyz
                print("Pose Jump:", before_stop, after_stop)
                self.xyz = before_stop
                self.go_straight(-1)
                self.stop()
                if right_wall:
                    turn_angle = math.pi / 2
                else:
                    turn_angle = -math.pi / 2
                self.turn(turn_angle, with_stop=True)
                self.go_straight(1.5)
                self.stop()
                self.turn(-turn_angle, with_stop=True)
                self.go_straight(1.5)
                self.stop()
                self.collision_detector_enabled = True
        self.scan = None
        return self.traveled_dist - start_dist, reason

    def return_home(self, timeout, home_threshold=None):
        if home_threshold is None:
            HOME_THRESHOLD = 5.0
        else:
            HOME_THRESHOLD = home_threshold
        SHORTCUT_RADIUS = 2.3
        MAX_TARGET_DISTANCE = 5.0
        MIN_TARGET_DISTANCE = 3.0
        assert(MAX_TARGET_DISTANCE > SHORTCUT_RADIUS) # Because otherwise we could end up with a target point more distant from home than the robot.
        print('Wait and get ready for return')
        self.send_speed_cmd(0, 0)
        self.wait(dt=timedelta(seconds=3.0))
        original_trace = copy.deepcopy(self.trace)
        self.trace.prune(SHORTCUT_RADIUS)
        self.wait(dt=timedelta(seconds=2.0))
        print('done.')
        home_position = self.trace.start_position()
        if home_position is None:
            return  # empty trace, no start -> we are at home
        start_time = self.sim_time_sec
        target_distance = MAX_TARGET_DISTANCE
        count_down = 0
        while distance3D(self.xyz, home_position) > HOME_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds():
            channel = self.update()
            if (channel == 'scan' and not self.flipped) or (channel == 'scan_back' and self.flipped) or (channel == 'scan360'):
                if target_distance == MIN_TARGET_DISTANCE:
                    target_x, target_y, target_z = original_trace.where_to(self.xyz, target_distance, self.trace_z_weight)
                else:
                    target_x, target_y, target_z = self.trace.where_to(self.xyz, target_distance, self.trace_z_weight)
                x, y = self.xyz[:2]
                yaw = (self.yaw + math.pi) if self.flipped else self.yaw
                desired_direction = normalizeAnglePIPI(math.atan2(target_y - y, target_x - x) - yaw)
                if self.flipped and self.joint_angle_rad:
                    desired_direction = normalizeAnglePIPI(desired_direction + sum(self.joint_angle_rad))
                if self.can_flip() and abs(desired_direction) > math.radians(95):  # including hysteresis
                    print('Flipping:', math.degrees(desired_direction))
                    self.flip()
                    continue

                safety, safe_direction = self.go_safely(desired_direction, allow_z_control=False)
#                print(self.time, self.xyz, (target_x, target_y, target_z), math.degrees(self.yaw), distance3D(self.xyz, (target_x, target_y, target_z)), target_distance, math.degrees(desired_direction), math.degrees(safe_direction))
                if safety < 0.2:
                    print(self.time, "Safety low!", safety, desired_direction)
                    target_distance = MIN_TARGET_DISTANCE
                    count_down = 300
                if count_down > 0:
                    count_down -= 1
                    if count_down == 0:
                        target_distance = MAX_TARGET_DISTANCE
                        print(self.time, "Recovery to original", target_distance)

                d = distance3D(self.xyz, [target_x, target_y, target_z])
                time_to_target = d / max(0.01, abs(self.speed_limit()))
                if self.slopes is not None:
                    # Climb at least as much as needed to fly over obstacles.
                    horizontal_distance_to_target = max(
                            0.01, math.hypot(target_x - self.xyz[0],
                                             target_y - self.xyz[1]))
                    desired_slope = math.atan2(target_z - self.xyz[2],
                                              horizontal_distance_to_target)
                    slope_idx = int(len(self.slopes) * (safe_direction - -math.pi) / (2 * math.pi))
                    if self.slopes[slope_idx] > desired_slope:
                        desired_slope = self.slopes[slope_idx]
                    desired_z_speed = math.tan(desired_slope) * horizontal_distance_to_target / time_to_target
                else:
                    desired_z_speed = (target_z - self.xyz[2]) / time_to_target
                self.bus.publish('desired_z_speed', desired_z_speed)

        print('return_home: dist', distance3D(self.xyz, home_position), 'time(sec)', self.sim_time_sec - start_time)
        self.bus.publish('desired_z_speed', None)

    def follow_trace(self, trace, timeout, max_target_distance=5.0, end_threshold=None, safety_limit=None, is_trace3d=False):
        print('Follow trace')
        if end_threshold is None:
            END_THRESHOLD = 2.0
        else:
            END_THRESHOLD = end_threshold
        start_time = self.sim_time_sec
        print('MD', self.xyz, distance3D(self.xyz, trace.trace[0]), trace.trace)
        while distance3D(self.xyz, trace.trace[0]) > END_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds():
            if self.update() in ['scan', 'scan360']:
                target_x, target_y, target_z = trace.where_to(self.xyz, max_target_distance, self.trace_z_weight)
                x, y, z = self.xyz
                yaw = (self.yaw + math.pi) if self.flipped else self.yaw
                desired_direction = normalizeAnglePIPI(math.atan2(target_y - y, target_x - x) - yaw)
                if self.flipped and self.joint_angle_rad:
                    desired_direction = normalizeAnglePIPI(desired_direction + sum(self.joint_angle_rad))
                if self.can_flip() and abs(desired_direction) > math.radians(95):  # including hysteresis
                    if is_trace3d:
                        self.bus.publish('desired_z_speed', 0)
                    print('Flipping:', math.degrees(desired_direction))
                    self.flip()
                else:
                    safety, safe_direction = self.go_safely(desired_direction, allow_z_control=False)
                    if safety_limit is not None and safety < safety_limit:
                        print('Danger! Safety limit for follow trace reached!', safety, safety_limit)
                        break

                    if is_trace3d:
                        d = distance3D(self.xyz, [target_x, target_y, target_z])
                        time_to_target = d / max(0.01, abs(self.speed_limit()))
                        if self.slopes is not None:
                            # Climb at least as much as needed to fly over obstacles.
                            horizontal_distance_to_target = max(
                                    0.01, math.hypot(target_x - self.xyz[0],
                                                     target_y - self.xyz[1]))
                            desired_slope = math.atan2(target_z - self.xyz[2],
                                                      horizontal_distance_to_target)
                            slope_idx = int(len(self.slopes) * (safe_direction - -math.pi) / (2 * math.pi))
                            if self.slopes[slope_idx] > desired_slope:
                                desired_slope = self.slopes[slope_idx]
                            desired_z_speed = math.tan(desired_slope) * horizontal_distance_to_target / time_to_target
                        else:
                            desired_z_speed = (target_z - self.xyz[2]) / time_to_target
                        self.bus.publish('desired_z_speed', desired_z_speed)

        print('End of follow trace(sec)', self.sim_time_sec - start_time)
        if is_trace3d:
            self.bus.publish('desired_z_speed', None)

    def register(self, callback):
        self.monitors.append(callback)
        return callback

    def unregister(self, callback):
        assert callback in self.monitors
        self.monitors.remove(callback)

    def on_pose3d(self, timestamp, data):
        xyz, rot = data
        self.orientation = rot  # quaternion
        ypr = quaternion.euler_zyx(rot)

        pose = (xyz[0], xyz[1], ypr[0])
        if self.last_position is not None:
            self.is_moving = (self.last_position != pose)
            dist = math.hypot(pose[0] - self.last_position[0], pose[1] - self.last_position[1])
            direction = ((pose[0] - self.last_position[0]) * math.cos(self.last_position[2]) +
                         (pose[1] - self.last_position[1]) * math.sin(self.last_position[2]))
            if direction < 0:
                dist = -dist
        else:
            dist = 0.0
        self.last_position = pose
        self.traveled_dist += dist
        x, y, z = xyz
        self.last_send_time = self.bus.publish('pose2d', [round(x * 1000), round(y * 1000),
                                    round(math.degrees(self.yaw) * 100)])
        if self.virtual_bumper is not None:
            if self.is_virtual:
                self.virtual_bumper.update_pose(timedelta(seconds=self.sim_time_sec), pose)
            else:
                self.virtual_bumper.update_pose(self.time, pose)
        self.xyz = tuple(xyz)
        if self.yaw_offset is None:
            self.yaw_offset = -ypr[0]
        tmp_yaw, self.pitch, self.roll = ypr
        self.yaw = tmp_yaw + self.yaw_offset
        self.trace.update_trace(self.xyz)
        self.loop_detector.add(self.xyz, rot, (self.flipped,))

    def on_acc(self, timestamp, data):
        acc = [x / 1000.0 for x in data]
        gacc = np.matrix([[0., 0., 9.80]])  # Gravitational acceleration.
        cos_pitch = math.cos(self.pitch)
        sin_pitch = math.sin(self.pitch)
        # TODO: Once roll is correct, incorporate it here too.
        egacc = np.matrix([  # Expected gravitational acceleration given known pitch.
            [cos_pitch, 0., sin_pitch],
            [0., 1., 0.],
            [-sin_pitch, 0., cos_pitch]
        ]) * gacc.T
        cacc = np.asarray(acc) - egacc.T  # Corrected acceleration (without gravitational acceleration).
        magnitude = math.hypot(cacc[0, 0], cacc[0, 1])
        # used to be 12 - see https://bitbucket.org/osrf/subt/issues/166/expected-x2-acceleration
        if magnitude > 200:  #18.0:
            print(self.time, 'Collision!', acc, 'reported:', self.collision_detector_enabled)
            if self.collision_detector_enabled:
                self.collision_detector_enabled = False
                raise Collision()

    def on_joint_angle(self, timestamp, data):
        # angles for articulated robot in 1/100th of degree
        self.joint_angle_rad = [math.radians(a/100) for a in data]

    def on_bumpers_front(self, timestamp, data):
        self.front_bumper = max(data)  # array of boolean values where True means collision

    def on_bumpers_rear(self, timestamp, data):
        self.rear_bumper = max(data)  # array of boolean values where True means collision

    def on_robot_name(self, timestamp, data):
        self.robot_name = data.decode('ascii')

    def on_waypoints(self, timestamp, data):
        self.waypoints = data

    def on_slopes(self, timestamp, data):
        self.slopes = [math.radians(a/10) for a in data]

    def on_whereabouts(self, timestamp, data):
        robot_name, (_, robot_pose) = data
        if robot_name != self.robot_name:
            self.whereabouts[robot_name] = robot_pose

    def update(self):
        packet = self.bus.listen()
        if packet is not None:
#            print('SubT', packet)
            timestamp, channel, data = packet
            if self.time is None or int(self.time.seconds)//60 != int(timestamp.seconds)//60:
                self.stdout(timestamp, '(%.1f %.1f %.1f)' % self.xyz if self.xyz is not None else '(xyz=None)', sorted(self.stat.items()))
                print(timestamp, list(('%.1f' % (v/100)) for v in self.voltage))
                self.stat.clear()

            self.time = timestamp

            if not self.is_virtual:
                self.sim_time_sec = self.time.total_seconds()

            self.stat[channel] += 1
            handler = getattr(self, "on_" + channel, None)
            if handler is not None:
                handler(timestamp, data)
            elif channel == 'scan' and not self.flipped:
                if self.last_send_time is not None and self.last_send_time - self.time > timedelta(seconds=0.1):
                    print('queue delay', self.last_send_time - self.time)
                self.scan = data
                if self.ref_scan is None or not equal_scans(self.scan, self.ref_scan, 200):
                    self.ref_scan = self.scan
                    self.ref_count = 0
                else:
                    self.ref_count += 1
                    if self.ref_count > 300:
                        print('Robot is stuck!', self.ref_count)
                        if self.collision_detector_enabled:
                            self.collision_detector_enabled = False
                            raise Collision()
                        self.ref_count = 0

                if self.local_planner is not None:
                    self.local_planner.update(data)
            elif channel == 'scan_back' and self.flipped:
                self.scan = data
                if self.local_planner is not None:
                    self.local_planner.update(data)
            elif channel == 'scan360':
                # reduce original 360 degrees scan to 270 degrees oriented forward or backward
                index45deg = int(round(len(data)/8))
                if self.flipped:
                    mid = len(data)//2
                    self.scan = (data[mid:]+data[:mid])[index45deg:-index45deg]
                else:
                    self.scan = data[index45deg:-index45deg]
                if self.local_planner is not None:
                    self.local_planner.update(self.scan)
            elif channel == 'sim_time_sec':
                self.sim_time_sec = data
            elif channel == 'voltage':
                self.voltage = data
            elif channel == 'emergency_stop':
                self.emergency_stop = data

            elif channel == 'cmd':
                self.lora_cmd = data

            for m in self.monitors:
                m(self)
            return channel

    def wait(self, dt, use_sim_time=False):  # TODO refactor to some common class
        if use_sim_time:
            start_sim_time_sec = self.sim_time_sec
            while self.sim_time_sec - start_sim_time_sec < dt.total_seconds():
                self.update()
        else:
            if self.time is None:
                self.update()
            start_time = self.time
            while self.time - start_time < dt:
                self.update()

    def stdout(self, *args, **kwargs):
        output = StringIO()
        print(*args, file=output, **kwargs)
        contents = output.getvalue().strip()
        output.close()
        self.bus.publish('stdout', contents)
        print(contents)

#############################################
    def system_nav_trace(self, path=None):
        """
        Navigate along line
        """
        x0, y0, z0 = self.xyz
        trace = Trace()
        trace.add_line_to((x0, y0, z0))
        if path is not None:
            for x, y in path:
                trace.add_line_to((x - x0, y - y0, z0))
        trace.reverse()
        self.follow_trace(trace, timeout=timedelta(seconds=120), max_target_distance=2.5, safety_limit=0.2)

    def robust_follow_wall(self, gap_size, wall_dist, right_wall=False, timeout=timedelta(hours=3), dist_limit=None,
            pitch_limit=None, roll_limit=None):
        """
        Handle multiple re-tries with increasing distance from the wall if necessary
        """
        allow_virtual_flip = self.symmetric
        wall_dist = self.wall_dist
        total_dist = 0.0
        start_time = self.sim_time_sec
        overall_timeout = timeout
        while self.sim_time_sec - start_time < overall_timeout.total_seconds():
            if self.sim_time_sec - start_time > overall_timeout.total_seconds():
                print('Total Timeout Reached', overall_timeout.total_seconds())
                break
            timeout = timedelta(seconds=overall_timeout.total_seconds() - (self.sim_time_sec - start_time))
            print('Current timeout', timeout)

            dist, reason = self.follow_wall(gap_size=gap_size, wall_dist=wall_dist, right_wall=right_wall, timeout=timeout,
                                    pitch_limit=self.limit_pitch, roll_limit=self.limit_roll)
            total_dist += dist
            if reason is None or reason in [REASON_LORA,]:
                break

            wall_dist += 0.2
            if not allow_virtual_flip:
                # Eduro not supported yet
                if reason in [REASON_VIRTUAL_BUMPER,]:
                    # virtual bumper already tried to backup a bit
                    continue
                # for large slope rather return home
                break

            # re-try with bigger distance
            print(self.time, "Re-try because of", reason)
            for repeat in range(2):
                # retreat a bit
                self.flip()
                self.follow_wall(gap_size=gap_size, wall_dist=wall_dist, right_wall=not right_wall, timeout=timedelta(seconds=30), dist_limit=2.0,
                    pitch_limit=self.return_limit_pitch, roll_limit=self.return_limit_roll)
                self.flip()

                dist, reason = self.follow_wall(gap_size=gap_size, wall_dist=wall_dist, right_wall=right_wall, timeout=timedelta(seconds=40), dist_limit=4.0,
                                        pitch_limit=self.limit_pitch, roll_limit=self.limit_roll)
                total_dist += dist
                if reason is None:
                    break
                if reason in [REASON_LORA, REASON_DIST_REACHED]:
                    break
                wall_dist += 0.2
            wall_dist = self.wall_dist
            if reason in [REASON_LORA,]:
                break

    def play_system_track(self):
        print("SubT Challenge Ver1!")
        try:
            with EmergencyStopMonitor(self):
                # wait for critical data
                while any_is_none(self.scan, self.xyz):
                    # self.xyz is initialized by pose3d
                    self.update()

                allow_virtual_flip = self.symmetric
                if distance(self.xyz, (0, 0)) > 0.1 or self.init_path is not None:
                    self.system_nav_trace(self.init_path)

#                self.go_straight(2.5)  # go to the tunnel entrance - commented our for testing
                wall_dist = self.wall_dist
                total_dist = 0.0
                start_time = self.sim_time_sec
                while self.sim_time_sec - start_time < self.timeout.total_seconds():
                    if self.sim_time_sec - start_time > self.timeout.total_seconds():
                        print('Total Timeout Reached', self.timeout.total_seconds())
                        break
                    timeout = timedelta(seconds=self.timeout.total_seconds() - (self.sim_time_sec - start_time))
                    print('Current timeout', timeout)

                    dist, reason = self.follow_wall(gap_size=self.gap_size, wall_dist=wall_dist, right_wall=self.use_right_wall, timeout=timeout,
                                            pitch_limit=self.limit_pitch, roll_limit=self.limit_roll)
                    total_dist += dist
                    if reason is None or reason in [REASON_LORA,]:
                        break

                    wall_dist += 0.2
                    if not allow_virtual_flip:
                        # Eduro not supported yet
                        if reason in [REASON_VIRTUAL_BUMPER,]:
                            # virtual bumper already tried to backup a bit
                            continue
                        # for large slope rather return home
                        break

                    # re-try with bigger distance
                    print(self.time, "Re-try because of", reason)
                    for repeat in range(2):
                        if allow_virtual_flip:
                            self.flip()
                        self.follow_wall(gap_size=self.gap_size, wall_dist=wall_dist, right_wall=not self.use_right_wall, timeout=timedelta(seconds=30), dist_limit=2.0,
                            pitch_limit=self.return_limit_pitch, roll_limit=self.return_limit_roll)
                        if allow_virtual_flip:
                            self.flip()

                        dist, reason = self.follow_wall(gap_size=self.gap_size, wall_dist=wall_dist, right_wall=self.use_right_wall, timeout=timedelta(seconds=40), dist_limit=4.0,
                                                pitch_limit=self.limit_pitch, roll_limit=self.limit_roll)
                        total_dist += dist
                        if reason is None:
                            break
                        if reason in [REASON_LORA, REASON_DIST_REACHED]:
                            break
                        wall_dist += 0.2
                    wall_dist = self.wall_dist
                    if reason in [REASON_LORA,]:
                        break

                if self.use_return_trace and self.local_planner is not None:
                    self.stdout(self.time, "Going HOME %.3f" % dist, reason)
                    self.return_home(2 * self.timeout, home_threshold=1.0)
                    self.send_speed_cmd(0, 0)
                else:
                    print(self.time, "Going HOME", reason)
                    if not allow_virtual_flip:
                        self.turn(math.radians(90), timeout=timedelta(seconds=20))
                        self.turn(math.radians(90), timeout=timedelta(seconds=20))
                    else:
                        self.flip()
                    self.robust_follow_wall(gap_size=self.gap_size, wall_dist=self.wall_dist, right_wall=not self.use_right_wall, timeout=3*self.timeout, dist_limit=3*total_dist,
                            pitch_limit=self.return_limit_pitch, roll_limit=self.return_limit_roll)

        except EmergencyStopException:
            print(self.time, "EMERGENCY STOP - terminating")
        self.send_speed_cmd(0, 0)
        self.wait(timedelta(seconds=3))
#############################################

    def go_to_entrance_VirtualTODOFIX(self):
        """
        Navigate to the base station tile end
        """
        trace = Trace()
        trace.update_trace(tuple(self.xyz))
        trace.add_line_to((-2.0, 0, self.height_above_ground))  # in front of the tunnel/entrance (Finals)
        if self.use_right_wall:
            entrance_offset = -0.5
        elif self.use_center:
            entrance_offset = 0
        else:
            entrance_offset = 0.5
        trace.add_line_to((3.0, entrance_offset, self.height_above_ground))  # 0.5m inside, towards the desired wall.
        trace.reverse()
        is_trace3d = self.height_above_ground > 0.0  # well, maybe there should be more obvious definition of ground/aerial vehicle
        safety_limit = None if is_trace3d else 0.2  # UGV may need some collision avoidance during leaving the starting area
                                                    # while the CoRo Pam drone generates fake artifacts near ground
        self.follow_trace(trace, timeout=timedelta(seconds=30), max_target_distance=2.5, safety_limit=safety_limit, is_trace3d=is_trace3d)

    def go_to_entrance(self):
        print("checking paused state")
        if self.pause_start_time is not None:
            print('PAUSED - waiting for release')
            while self.pause_start_time is not None:
                self.update()
        print("go_to_entrance - System HACK")
        if distance(self.xyz, (0, 0)) > 0.1 or self.init_path is not None:
            print('init_path is used')
            self.system_nav_trace(self.init_path)
        self.send_speed_cmd(0, 0)

    def play_virtual_part_explore(self):
        start_time = self.sim_time_sec
        for loop in range(100):
            self.collision_detector_enabled = True
            if self.sim_time_sec - start_time > self.timeout.total_seconds():
                print('Total Timeout Reached', self.timeout.total_seconds())
                break
            timeout = timedelta(seconds=self.timeout.total_seconds() - (self.sim_time_sec - start_time))
            print('Current timeout', timeout)

            dist, reason = self.follow_wall(gap_size=self.gap_size, wall_dist=self.wall_dist, right_wall=self.use_right_wall,
                                timeout=timeout, pitch_limit=self.limit_pitch, roll_limit=None)
            self.collision_detector_enabled = False
            if reason == REASON_VIRTUAL_BUMPER:
                assert self.virtual_bumper is not None
                self.virtual_bumper.reset_counters()

                # the robot ended in cycle probably
                self.return_home(timedelta(seconds=10))

                # try something crazy if you do not have other ideas ...
                before_center = self.use_center
                self.use_center = True
                dist, reason = self.follow_wall(gap_size=self.gap_size, wall_dist=self.wall_dist, right_wall=self.use_right_wall,
                                    timeout=timedelta(seconds=60), pitch_limit=self.limit_pitch, roll_limit=None)
                self.use_center = before_center
                if reason is None or reason != REASON_PITCH_LIMIT:
                    continue
            elif reason == REASON_LOOP:
                # Smaller wall_dist to reduce the chance that we miss the opening again that we missed before,
                # which made us end up in a loop.
                dist, reason = self.follow_wall(gap_size=self.gap_size, wall_dist=self.wall_dist*0.75, right_wall=not self.use_right_wall,
                                    timeout=timedelta(seconds=5), pitch_limit=self.limit_pitch, roll_limit=None, detect_loop=False)
                if reason is None:
                    continue

            if reason is None or reason != REASON_PITCH_LIMIT:
                break
            self.stdout(self.time, "Microstep HOME %d %.3f" % (loop, dist), reason)
            self.go_straight(-0.3, timeout=timedelta(seconds=10))
            self.return_home(timedelta(seconds=10))

        self.stdout(self.time, "Explore phase finished %.3f" % dist, reason)

    def play_virtual_part_map_and_explore_frontiers(self):
        start_time = self.sim_time_sec
        while self.sim_time_sec - start_time < self.timeout.total_seconds():
            if self.waypoints is not None:
                tmp_trace = Trace()
                tmp_trace.trace = self.waypoints
                self.waypoints = None
                tmp_trace.reverse()
                try:
                    with NewWaypointsMonitor(self) as wm:
                        self.follow_trace(tmp_trace, timeout=timedelta(seconds=10),
                                          max_target_distance=1.0, end_threshold=0.5, is_trace3d=True)
                except NewWaypointsException:
                    pass
                self.send_speed_cmd(0, 0)
            else:
                self.update()

    def play_virtual_part_return(self, timeout):
        deadline = timedelta(seconds=self.sim_time_sec) + timeout
        try:
            with NotMovingMonitor(self, radius=10.0, sim_time_sec_period=20):
                self.return_home(timeout)
        except NotMovingException:
            self.use_right_wall = not self.use_right_wall
            self.use_center = False # Better use one of the walls.
            self.timeout = deadline - timedelta(seconds=self.sim_time_sec)
            self.loop_detector = LoopDetector()  # The robot may need to go where it already was and that is OK.
            try:
                with HomeReachedMonitor(self, radius=5):
                    self.play_virtual_part_explore()
            except HomeReachedException:
                timeout = deadline - timedelta(seconds=self.sim_time_sec)
                # Try to not block the entrance.
                self.return_home(timeout)
        self.send_speed_cmd(0, 0)
        self.wait(timedelta(seconds=10), use_sim_time=True)
        self.stdout('Final xyz:', self.xyz)
        self.stdout('Final xyz (DARPA coord system):', self.xyz)

    def play_virtual_track(self):
        self.stdout("SubT Challenge Ver133!")
        self.stdout("Waiting for robot_name ...")
        while self.robot_name is None:
            self.update()
        self.stdout('robot_name:', self.robot_name)

        # wait for critical data
        while any_is_none(self.scan, self.xyz):
            # self.xyz is initialized by pose3d
            self.update()

        steps = parse_robot_name(self.robot_name)
        times_sec = [duration for action, duration in steps if action != 'home' and not action.startswith('enter')]
        self.stdout('Using times', times_sec)

        for action, duration, in steps:
            if action == 'wait':
                self.stdout(f'Wait for {duration} seconds')
                self.wait(timedelta(seconds=duration), use_sim_time=True)

            elif action.startswith('enter'):
                self.use_right_wall = (action == 'enter-right')
                self.use_center = (action == 'enter-center')
                self.go_to_entrance()

            elif action in ['left', 'right', 'center']:
                self.timeout = timedelta(seconds=duration)
                self.use_right_wall = (action == 'right')
                self.use_center = (action == 'center')
                self.play_virtual_part_explore()

            elif action == 'explore':
                self.timeout = timedelta(seconds=duration)
                self.play_virtual_part_map_and_explore_frontiers()

            elif action == 'home':
                self.play_virtual_part_return(timedelta(seconds=duration))
            else:
                assert False, action  # unknown action

        self.wait(timedelta(seconds=30), use_sim_time=True)

#############################################

    def play(self):
        if self.is_virtual:
            try:
                with EmergencyStopMonitor(self):
                    return self.play_virtual_track()
            except EmergencyStopException:
                print(self.time, "EMERGENCY STOP - terminating")
            self.send_speed_cmd(0, 0)
            self.wait(timedelta(seconds=3))
        else:
            return self.play_system_track()

    def start(self):
        self.thread = threading.Thread(target=self.play)
        self.thread.start()

    def is_alive(self):
        return self.thread.is_alive()

    def request_stop(self):
        self.bus.shutdown()

    def join(self, timeout=None):
        self.thread.join(timeout)

def main():
    import argparse
    from osgar.lib.config import config_load

    parser = argparse.ArgumentParser(description='SubT Challenge')
    parser.add_argument('--use-old-record', help="use old osgar.record instead of zmqrouter", action='store_true')
    subparsers = parser.add_subparsers(help='sub-command help', dest='command')
    subparsers.required = True
    parser_run = subparsers.add_parser('run', help='run on real HW')
    parser_run.add_argument('config', nargs='+', help='configuration file')
    parser_run.add_argument('--note', help='add description')
    parser_run.add_argument('--gap-size', help='minimum gap the robot goeas into (default: %(default)sm)', default=1.0, type=float)
    parser_run.add_argument('--wall-dist', help='distance for wall following (default: %(default)sm)', default=1.0, type=float)
    parser_run.add_argument('--side', help='which side to follow', choices=['left', 'right', 'auto'], required=True)
    parser_run.add_argument('--speed', help='maximum speed (default: from config)', type=float)
    parser_run.add_argument('--timeout', help='seconds of exploring before going home (default: %(default)s)',
                            type=int, default=10*60)
    parser_run.add_argument('--log', nargs='?', help='record log filename')
    parser_run.add_argument('--init-path', help='inital path to be followed from (0, 0). 2D coordinates are separated by ;')
    parser_run.add_argument('--start-paused', dest='start_paused', action='store_true',
                            help='start robot Paused and wait for LoRa Continue command')

    parser_replay = subparsers.add_parser('replay', help='replay from logfile')
    parser_replay.add_argument('logfile', help='recorded log file')
    parser_replay.add_argument('--force', '-F', dest='force', action='store_true', help='force replay even for failing output asserts')
    parser_replay.add_argument('--config', nargs='+', help='force alternative configuration file')
    parser_replay.add_argument('--debug', help="print debug info about I/O streams", action='store_true')
    args = parser.parse_args()

    if args.use_old_record:
        from osgar.record import record
    else:
        from osgar.zmqrouter import record

    if args.command == 'replay':
        from osgar.replay import replay
        args.module = 'app'
        app = replay(args, application=SubTChallenge)
        app.play()

    elif args.command == 'run':
        import logging
        logging.basicConfig(
            level=logging.DEBUG,
            format='%(asctime)s %(name)-16s %(levelname)-8s %(message)s',
        )
        # To reduce latency spikes as described in https://morepypy.blogspot.com/2019/01/pypy-for-low-latency-systems.html.
        # Increased latency leads to uncontrolled behavior and robot either missing turns or hitting walls.
        # Disabled garbage collection needs to be paired with gc.collect() at place(s) that are not time sensitive.
        gc.disable()

        cfg = config_load(*args.config, application=SubTChallenge)

        # apply overrides from command line
        cfg['robot']['modules']['app']['init']['gap_size'] = args.gap_size
        cfg['robot']['modules']['app']['init']['wall_dist'] = args.wall_dist
        if args.side == 'auto':
            cfg['robot']['modules']['app']['init']['right_wall'] = 'auto'
        else:
            cfg['robot']['modules']['app']['init']['right_wall'] = args.side == 'right'
            cmd = 'R' if args.side == 'right' else 'L'
            cfg['robot']['modules']['app']['init']['robot_name'] = f'A{args.timeout}{cmd}'
        cfg['robot']['modules']['app']['init']['timeout'] = args.timeout
        if args.init_path is not None:
            cfg['robot']['modules']['app']['init']['init_path'] = args.init_path

        if args.speed is not None:
            cfg['robot']['modules']['app']['init']['max_speed'] = args.speed

        cfg['robot']['modules']['app']['init']['start_paused'] = args.start_paused

        prefix = os.path.basename(args.config[0]).split('.')[0] + '-'
        record(cfg, prefix, args.log)

if __name__ == "__main__":
    main()

# vim: expandtab sw=4 ts=4

max_stars_repo_name: robotika/osgar

max_issues_repo_name: robotika/osgar

max_forks_repo_name: robotika/osgar


text:
"""
All functions combined
"""
import math

import cv2
import matplotlib.pyplot as plt
import numpy as np

HARRIS = False
FEATURE_EXTRACTOR = 'brisk'  # 'sift'|'surf'|'brisk'|'orb'
FEATURE_MATCHER = 'bf'  # 'bf'|'knn'
NUM_GOOD_MATCHES = 200
LOWES_RATIO = 0.75
SCALING = 20  # Percent scale down
# MARKERS_PATHS = ['../markers/0.png', '../markers/1.png', '../markers/2.png']
MARKERS_PATHS = ['../../markers/0.png', '../../markers/2.png']
REF_IMAGE1 = np.array([[0, 0],
                      [0, 700],
                      [700, 700],
                      [700, 0]], dtype=np.float32)
REF_IMAGE2 = np.array([[0, 0],
                      [0, 700],
                      [700, 700],
                      [700, 0]], dtype=np.float32)

REF_IMAGE3 = np.array([[0, 0],
                      [0, 700],
                      [700, 700],
                      [700, 0]], dtype=np.float32)

ROT = np.array([[0,1],[3,2]])

def do_everything(FRAMED, REF_IMAGES, OBJ, CAM_MAT, REACHED_X, REACHED_Y):
    FRAME = convert_to_grayscale(FRAMED)
    # ret, thresh = cv2.threshold(FRAME,127,255,cv2.THRESH_BINARY)
    FRAME = cv2.GaussianBlur(FRAME,(5,5),0)
    thresh = cv2.adaptiveThreshold(FRAME,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 11, 2)
    # cv2.imshow("thresh", thresh)
    # print(thresh)
    contours, hierarchy = cv2.findContours(thresh, 1, 2)
    area_contour = [contour for contour in contours if cv2.contourArea(contour) >= 500]
    rects = [cv2.minAreaRect(i) for i in area_contour]
    boxes = [np.int0(cv2.boxPoints(i)) for i in rects]
    warped = [four_point_transform(FRAME, box) for box in boxes]
    selected_markers = compare_markers(warped, REF_IMAGES)

    # cv2.imshow("0", selected_markers[0][0])
    # cv2.imshow("1", selected_markers[1][0])
    for sel in selected_markers:
        try:
            if sel[3] <= 4:
                cv2.drawContours(FRAMED,[boxes[sel[2]]],-1,(0,0,255),3)
        except:
            pass

    MATCH_DATA = [None, None]
    try:
        if selected_markers[0][3] <= 4:
            ROT3 = np.rot90(ROT)
            REF_IMAGE3[0] = REF_IMAGE1[ROT3[0,0]]
            REF_IMAGE3[1] = REF_IMAGE1[ROT3[0,1]]
            REF_IMAGE3[2] = REF_IMAGE1[ROT3[1,1]]
            REF_IMAGE3[3] = REF_IMAGE1[ROT3[1,0]]
            h1 = get_homography_from_corners(boxes[selected_markers[0][2]], REF_IMAGE3)[0]

        h2 = get_homography_from_corners(boxes[selected_markers[1][2]], REF_IMAGE2)[0]
        MATCH_DATA = [h1, h2]
    except:
        pass

    if MATCH_DATA[0] is not None and MATCH_DATA[1] is not None:
        HOMOGRAPHY1 = MATCH_DATA[0]
        HOMOGRAPHY2 = MATCH_DATA[1]
        corner1 = boxes[selected_markers[0][2]]
        corner2 = boxes[selected_markers[1][2]]

        PROJ_MAT1 = get_matrix(CAM_MAT, HOMOGRAPHY1)

        DIST = calculate_dist_corners(corner1, corner2)
        DIST_X = DIST[0]
        DIST_Y = DIST[1]

        STEP_X = DIST_X/10
        STEP_Y = DIST_Y/10

        if abs(REACHED_X) >= abs(DIST_X) or abs(REACHED_Y) >= abs(DIST_Y):
            REACHED_X = 0
            REACHED_Y = 0
        else:
            REACHED_X += STEP_X
            REACHED_Y += STEP_Y

        TRANS = np.array(
            [[1, 0, REACHED_X], [0, 1, REACHED_Y], [0, 0, 1]])
        PROJ_MAT = np.dot(TRANS, PROJ_MAT1)
        FRAMED = render(FRAMED, OBJ, PROJ_MAT, REF_IMAGE1, False)
    return REACHED_X, REACHED_Y

def get_middle(arr):
    n = np.array(arr.shape) / 2.0
    n_int = np.int0(n)
    if n[0] % 2 == 1 and n[1] %2 == 1:
        return arr[n_int[0], n_int[1]]
    elif n[0] %2 == 0 and n[1] %2 == 0:
        return np.average(arr[n_int[0]:n_int[0] + 2, n_int[1]:n_int[1] + 2])
    elif n[0] %2 == 1 and n[1] %2 == 0:
        return np.average(arr[n_int[0], n_int[1]:n_int[1]+2])
    else:
        return np.average(arr[n_int[0]:n_int[0]+2, n_int[1]])

def get_binary(images, divsions):
    # threshed = [cv2.threshold(warp, 127, 255, cv2.THRESH_BINARY)[1]/255 for warp in images]
    threshed = [cv2.GaussianBlur(thresh,(5,5),0) for thresh in images]
    # threshed = images
    # threshed = [cv2.adaptiveThreshold(warp, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, 11, 2) for warp in threshed]
    threshed = [cv2.threshold(warp, 0, 255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)[1]/255 for warp in threshed]
    mats = []
    for thr in threshed:
        shap = thr.shape
        jump = np.int0(np.array(shap)/divsions)
        mat = np.zeros([divsions,divsions])
        for i in range(divsions):
            for j in range(divsions):
                try:
                    avg = get_middle(thr[i*jump[0]:(i+1)*jump[0], j*jump[1]:(j+1)*jump[1]])
                except:
                    avg = 0
                # avg = np.average(thr[i*jump[0]:(i+1)*jump[0], j*jump[1]:(j+1)*jump[1]])
                if avg >= 0.5:
                    mat[i][j] = 1
                else:
                    mat[i][j] = 0
        mats.append(mat)
    return mats

def compare_markers(matrices, ref_images):
    thresh = get_binary(matrices, 7)
    refs = get_binary([convert_to_grayscale(ref) for ref in ref_images], 7)
    ret = []
    for ref in refs:
        mini = np.sum(np.absolute(refs[0]-thresh[0]))
        rot = (thresh[0], ref, 0, mini)
        for thr in range(0, len(thresh)):
            for i in range(4):
                su = np.sum(np.absolute(np.rot90(ref, i)-thresh[thr]))
                # print("su, mini ", su, mini)
                if su < mini:
                    if len(ret) > 0 and ret[0][2] != thr:
                        mini = su
                        rot = (thresh[thr], i, thr, mini)
                    elif len(ret) == 0:
                        mini = su
                        rot = (thresh[thr], i, thr, mini)
        ret.append(rot)
    return ret

def order_points(pts):
    rect = np.zeros((4, 2), dtype = "float32")
    s = pts.sum(axis = 1)
    rect[0] = pts[np.argmin(s)]
    rect[2] = pts[np.argmax(s)]
    diff = np.diff(pts, axis = 1)
    rect[1] = pts[np.argmin(diff)]
    rect[3] = pts[np.argmax(diff)]
    return rect

def four_point_transform(image, pts):
    rect = order_points(pts)
    (tl, tr, br, bl) = rect

    widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2))
    widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2))
    maxWidth = max(int(widthA), int(widthB))

    heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2))
    heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2))
    maxHeight = max(int(heightA), int(heightB))

    dst = np.array([
        [0, 0],
        [maxWidth - 1, 0],
        [maxWidth - 1, maxHeight - 1],
        [0, maxHeight - 1]], dtype = "float32")

    M = cv2.getPerspectiveTransform(rect, dst)
    warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight))

    return warped

def create_matcher():
    """
    Create different types of matchers to match descriptors.
    """
    cross_check = bool(FEATURE_MATCHER == 'bf')

    if FEATURE_EXTRACTOR in ('sift', 'surf'):
        matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=cross_check)
    else:
        matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=cross_check)
    return matcher

MATCHER = create_matcher()

def average(lst):
    """
    Average x.distance from a list of x.
    """
    sum_ = 0
    for i in lst:
        sum_ += i.distance
    if len(lst) == 0:
        return 10
    return sum_ / len(lst)

def scale_image(img):
    """
    Scale down images
    """
    img = cv2.resize(img, ((
        (img.shape[1])*SCALING)//100, (img.shape[0]*SCALING)//100))
    return img

def load_image(image_path, scale=True):
    """
    Load image
    """
    image = cv2.imread(image_path)
    if SCALING != 100 and scale:
        image = scale_image(image)
    return image

def convert_to_grayscale(image):
    """
    Convert images from RGB to Grayscale
    """
    return cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)

def get_harris_keypoints(gray):
    """
    Returns corners found using harris corner detection algorithm with sub pixel accuracy.
    """
    # find Harris corners
    gray = np.float32(gray)
    dst = cv2.cornerHarris(gray, 2, 3, 0.04)
    dst = cv2.dilate(dst, None)
    _, dst = cv2.threshold(dst, 0.01*dst.max(), 255, 0)
    dst = np.uint8(dst)

    # find centroids
    _, _, _, centroids = cv2.connectedComponentsWithStats(dst)

    # define the criteria to stop and refine the corners
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
    corners = cv2.cornerSubPix(gray, np.float32(
        centroids), (5, 5), (-1, -1), criteria)
    keypoints = [cv2.KeyPoint(crd[0], crd[1], 13) for crd in corners]

    return keypoints

def get_descriptors(gray_image):
    """
    Generate descriptors from image according to various descriptor modules
    """
    if FEATURE_EXTRACTOR == 'surf':
        descriptor = cv2.xfeatures2d.SURF_create()
    elif FEATURE_EXTRACTOR == 'sift':
        descriptor = cv2.xfeatures2d.SIFT_create()
    elif FEATURE_EXTRACTOR == 'brisk':
        descriptor = cv2.BRISK_create()
    elif FEATURE_EXTRACTOR == 'orb':
        descriptor = cv2.ORB_create()

    if HARRIS:
        kps = get_harris_keypoints(gray_image)
        keypoints, des = descriptor.compute(gray_image, kps)
        return (keypoints, des)

    return descriptor.detectAndCompute(gray_image, None)

def get_matches(kp1_loc, dsc1_loc, kp2_loc, dsc2_loc):
    """
    Get the matching descriptors according to feature matcher.
    """

    if FEATURE_MATCHER == 'bf':
        raw_matches = MATCHER.match(dsc1_loc, dsc2_loc)
        raw_matches.sort(key=lambda x: x.distance)
        matches_loc = raw_matches[:NUM_GOOD_MATCHES]
        # print("Brute Force #matches = ", len(raw_matches),
        #       " and avd_dist: ", average(matches_loc))

    else:
        raw_matches = MATCHER.knnMatch(dsc1_loc, dsc2_loc, 2)
        # print("KNN #matches = ", len(raw_matches))
        matches_loc = []
        for m_val, n_val in raw_matches:
            if m_val.distance < n_val.distance * LOWES_RATIO:
                matches_loc.append(m_val)

    points1_loc = np.zeros((len(matches_loc), 2), dtype=np.float32)
    points2_loc = np.zeros((len(matches_loc), 2), dtype=np.float32)

    for i, match in enumerate(matches_loc):
        points1_loc[i, :] = kp1_loc[match.queryIdx].pt
        points2_loc[i, :] = kp2_loc[match.trainIdx].pt

    return average(matches_loc), points1_loc, points2_loc, matches_loc

def display_image(img):
    """
    Show an image
    """
    plt.imshow(img)
    plt.show()

def draw_harris_kps(img):
    """
    Draw an image and its harris keypoints
    """

    gray_img = convert_to_grayscale(img)
    kps = get_harris_keypoints(gray_img)
    img_new = cv2.drawKeypoints(img, kps, None, color=(255, 0, 0))
    return img_new

def display_image_with_matches(img1_loc, kps1_loc, img2_loc, kps2_loc, matches_loc):
    """
    Display 2 images and their keypoints connected by lines side by side
    """
    img = cv2.drawMatches(img1_loc, kps1_loc, img2_loc, kps2_loc, matches_loc,
                          None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    plt.imshow(img)
    plt.show()

def get_hom(img1_descriptors, img2):
    """
    Get homography between 2 images
    """
    imb = convert_to_grayscale(img2)
    desca = img1_descriptors
    descb = get_descriptors(imb)
    (kp1, dsc1), (kp2, dsc2) = desca, descb
    avg_dist, points1, points2, matches = get_matches(kp1, dsc1, kp2, dsc2)
    hom, _ = cv2.findHomography(points1, points2, cv2.RANSAC)
    return hom, matches, avg_dist, kp2

def get_homography_from_corners(corners, ref_image):
    """
    Get homography using corners of reference image
    """
    pts_dst = np.array([[corners[0][0], corners[0][1]],
                        [corners[1][0], corners[1][1]],
                        [corners[2][0], corners[2][1]],
                        [corners[3][0], corners[3][1]]])
    pts_src = ref_image

    homography, status = cv2.findHomography(pts_src, pts_dst)
    return homography, status

def get_matrix(camera_params, homography, translate=None):
    """
    Using camera params and homography matrix get projection matrix
    """
    r_t = np.dot(np.linalg.inv(camera_params), homography)
    r_1 = r_t[:, 0]
    r_2 = r_t[:, 1]
    t_vec = r_t[:, 2]
    norm = math.sqrt(np.linalg.norm(r_1, 2) * np.linalg.norm(r_2, 2))
    r_1 = r_1/norm
    r_2 = r_2/norm
    t_vec = t_vec/norm
    c_val = r_1+r_2
    p_val = np.cross(r_1, r_2)
    d_val = np.cross(c_val, p_val)
    r1_ = (1/math.sqrt(2))*(c_val/np.linalg.norm(c_val, 2) +
                            d_val/np.linalg.norm(d_val, 2))
    r2_ = (1/math.sqrt(2))*(c_val/np.linalg.norm(c_val, 2) -
                            d_val/np.linalg.norm(d_val, 2))
    r3_ = np.cross(r1_, r2_)
    proj = np.stack((r1_, r2_, r3_, t_vec)).T

    if translate is None:
        return np.dot(camera_params, proj)

    return np.dot(camera_params, np.dot(translate, proj))

def render(img, obj, projection, model, color=False):
    """
    Render a loaded obj model into the current video frame
    """
    old_img = img.copy()
    vertices = obj.vertices
    scale_matrix = np.eye(3) * 3
    height, width = model.shape

    for face in obj.faces:
        face_vertices = face[0]
        points = np.array([vertices[vertex - 1] for vertex in face_vertices])
        points = np.dot(points, scale_matrix)
        # render model in the middle of the reference surface. To do so,
        # model points must be displaced
        # print(points.shape)
        points = np.array(
            [[p[0] + width / 2, p[1] + height / 2, p[2]] for p in points])
        dst = cv2.perspectiveTransform(points.reshape((-1, 1, 3)), projection)
        imgpts = np.int32(dst)
        mini_img = np.min(imgpts, axis=0)[0]
        maxi_img = np.max(imgpts, axis=0)[0]
        mini_canvas = [0, 0]
        maxi_canvas = [img.shape[0], img.shape[1]]

        if (mini_img[0] < mini_canvas[0]) or (mini_img[1] < mini_canvas[1]) or (maxi_img[0] > maxi_canvas[0]) or (maxi_img[1] > maxi_canvas[1]):
            return old_img

        if color is False:
            cv2.fillConvexPoly(img, imgpts, (137, 27, 211))
        else:
            color = hex_to_rgb(face[-1])
            color = color[::-1]  # reverse
            cv2.fillConvexPoly(img, imgpts, color)

    return img

def get_camera_params():
    """
    Return a default camera parameter matrix
    """
    return np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]])

def load_ref_images(num_img=2):
    """
    Load visual markers reference images
    """

    ref_imgs = []

    for marker_path in MARKERS_PATHS:
        ref_imgs.append(load_image(marker_path, False))

    ref_imgs = ref_imgs[:num_img]

    ref_descriptors = []

    for img in ref_imgs:
        img_grayscale = convert_to_grayscale(img)
        ref_descriptors.append(get_descriptors(img_grayscale))

    return ref_imgs, ref_descriptors

def find_homographies(reference_descriptors, frame):
    """
    Find homography for each reference image
    """
    match_data = []  # (homography,matches,avg_dist)

    for descriptor in reference_descriptors:
        match_data.append(get_hom(descriptor, frame))

    return match_data

def draw_rectangle(homography, ref_img, frame, color=255):
    """
    Draw rectangles around each found visual marker
    """
    height, width, _ = ref_img.shape
    pts = np.array([[0, 0], [0, height - 1], [width - 1, height - 1],
                    [width - 1, 0]], dtype='float32').reshape((-1, 1, 2))
    dst = cv2.perspectiveTransform(pts, homography)
    frame = cv2.polylines(
        frame, [np.int32(dst)], True, color, 3, cv2.LINE_AA)
    return frame, dst

def calculate_dist_matches(kp1, matches1, kp2, matches2):
    """
    Function to calculate distance between two planes after projecting using homographies.
    """
    minimum = min(len(matches1), len(matches2))
    points1 = np.zeros((minimum, 2), dtype=np.float32)
    points2 = np.zeros((minimum, 2), dtype=np.float32)

    for i, match in enumerate(matches1[:minimum]):
        points1[i, :] = kp1[match.trainIdx].pt

    for i, match in enumerate(matches2[:minimum]):
        points2[i, :] = kp2[match.trainIdx].pt

    # print("mats: ", points2-points1)
    a = np.average(points2-points1, axis=0)
    # print("average: ", a)
    return a

def calculate_dist_corners(corner1, corner2):
    """
    Function to calculate distance between two planes after projecting using homographies.
    """
    dst = np.average(corner2-corner1, axis=0)
    return dst

def display_image_with_matched_keypoints(img1_loc, kps1_loc):
    """
    Display 2 images and their keypoints side by side
    """
    _, (ax1, ax2) = plt.subplots(nrows=1, ncols=2,
                                 figsize=(20, 8), constrained_layout=False)
    return cv2.drawKeypoints(img1_loc, kps1_loc, None, color=(255, 0, 0))

def init_game(size_window):
    """
    Init game of ping pong.
    """
    print(size_window)
    game_obj = {
        'pos': (size_window[1]/2, size_window[0]/2),
        'velocity': (10, 10),
        'rudder1_pos': (0.05*size_window[1], size_window[0]/2),
        'rudder2_pos': (0.95*size_window[1], 1.5*size_window[0]/2),
        'score1': 0,
        'score2': 0,
    }

    return game_obj

def draw_game(frame, size, game_obj):
    """
    Draw game using game_object on frame.
    """
    pos_float = game_obj['pos']
    pos_int = ((int)(pos_float[0]), (int)(pos_float[1]))
    frame = cv2.circle(frame, pos_int, 15, (0, 0, 255), -1)
    frame = cv2.rectangle(frame, (15, 15), (640-15, 480-15), (0, 255, 0), 1)

    rudder_len = 50
    rudder_thickness = 2

    player1_bat_center = game_obj['rudder1_pos']
    player2_bat_center = game_obj['rudder2_pos']

    p1_start = (int(
        player1_bat_center[0]-rudder_thickness), int(player1_bat_center[1]-rudder_len))
    p1_end = (
        int(player1_bat_center[0]+rudder_thickness), int(player1_bat_center[1]+rudder_len))

    p2_start = (
        int(player2_bat_center[0]-rudder_thickness), int(player2_bat_center[1]-rudder_len))
    p2_end = (
        int(player2_bat_center[0]+rudder_thickness), int(player2_bat_center[1]+rudder_len))

    frame = cv2.rectangle(frame, p1_start, p1_end, (0, 255, 0), -1)
    frame = cv2.rectangle(frame, p2_start, p2_end, (0, 255, 0), -1)

    size_x = size[1]
    size_y = size[0]

    font = cv2.FONT_HERSHEY_SIMPLEX
    bottomLeftCornerOfText = (10, size_y-10)
    bottomRightCornerOfText = (size_x-30, size_y-10)
    fontScale = 1
    fontColor = (255, 255, 255)
    lineType = 2

    cv2.putText(frame, str(game_obj['score1']),
                bottomLeftCornerOfText,
                font,
                fontScale,
                fontColor,
                lineType)

    cv2.putText(frame, str(game_obj['score2']),
                bottomRightCornerOfText,
                font,
                fontScale,
                fontColor,
                lineType)
    return frame

def update_game(game_obj, size, center1, center2):
    """
    Update game state
    """
    new_game_obj = game_obj.copy()

    if center1 is not None:
        new_game_obj['rudder1_pos'] = center1
    if center2 is not None:
        new_game_obj['rudder2_pos'] = center2

    # Check if hitting corner
    init_vel = new_game_obj['velocity']
    if new_game_obj['pos'][1] >= 480-15 or new_game_obj['pos'][1] <= 15:
        new_game_obj['velocity'] = (init_vel[0], -1*init_vel[1])
    if new_game_obj['pos'][0] >= 640-15:
        new_game_obj['pos'] = (size[1]/2, size[0]/2)
        new_game_obj['velocity'] = (-1.05*abs(new_game_obj['velocity'][0]),
                                    1.05*abs(new_game_obj['velocity'][1]))
        new_game_obj['score1'] += 1
    elif new_game_obj['pos'][0] <= 15:
        new_game_obj['pos'] = (size[1]/2, size[0]/2)
        new_game_obj['score2'] += 1
        new_game_obj['velocity'] = (1.05*abs(new_game_obj['velocity'][0]),
                                    -1.05*abs(new_game_obj['velocity'][1]))
    elif 0 <= new_game_obj['pos'][0]-new_game_obj['rudder1_pos'][0] <= 17 and new_game_obj['rudder1_pos'][1]-(50+15) < new_game_obj['pos'][1] < new_game_obj['rudder1_pos'][1] + 50+15:
        new_game_obj['velocity'] = (-1*init_vel[0], init_vel[1])
    elif 0 <= new_game_obj['rudder2_pos'][0] - new_game_obj['pos'][0] <= 17 and new_game_obj['rudder2_pos'][1]-(50+15) < new_game_obj['pos'][1] < new_game_obj['rudder2_pos'][1]+(50+15):
        init_vel = new_game_obj['velocity']
        new_game_obj['velocity'] = (-1*init_vel[0], init_vel[1])

    new_game_obj['pos'] = (new_game_obj['pos'][0] + new_game_obj['velocity']
                           [0], new_game_obj['pos'][1] + new_game_obj['velocity'][1])

    # print(new_game_obj)
    return new_game_obj