from Game import YEET as Game
from NNet import NNetWrapper as nn
from dotted_dict import DottedDict as dotdict
#from multiprocessing import freeze_support
import os, logging, copy, time, sys, psutil
from collections import deque
from MCTS import MCTS
import numpy as np
from utils import Bar, AverageMeter
from pickle import Pickler, Unpickler
import pickle
from random import shuffle
import multiprocessing as mp
from multiprocessing import current_process, Pool
from concurrent.futures import ProcessPoolExecutor, as_completed
from tqdm import tqdm
import functools
from utils.helper import *

args = dotdict({
    'numIters': 100,
    'numEps': 2,
    'tempThreshold': 15,    # degree of exploration in MCTS.getActionProb(). switch from temperature=1 to temperature=0 after this episode step
    'maxlenOfQueue': 200000,
    'numMCTSSims': 25,      # 25    # TODO: much more sims needed?
    'cpuct': 2,             # degree of exploration for upper confidence bound in MCTS.search() => TODO: try 2?

    'modelspath': './models/',
    'examplespath': './examples/',
    'numThreads': 1 #psutil.cpu_count(),
})

class Coach:
    """
    This class executes the self-play + learning. It uses the functions defined
    in Game and NeuralNet. args are specified in main.py.
    """
    def __init__(self, game, nnet, args):
        self.game = game
        self.nnet = nnet
        self.pnet = self.nnet.__class__()  # the competitor network
        self.args = args
        self.mcts = MCTS(self.game, self.nnet, self.args)
        self.trainExamplesHistory = []    # history of examples from args.numItersForTrainExamplesHistory latest iterations
        self.skipFirstSelfPlay = False # can be overridden in loadTrainExamples()

    def executeEpisode(self, x):
        """
        This function executes one episode of self-play, starting with player 1.
        As the game is played, each turn is added as a training example to
        trainExamples. The game is played till the game ends. After the game
        ends, the outcome of the game is used to assign values to each example
        in trainExamples.

        It uses a temp=1 if episodeStep < tempThreshold, and thereafter
        uses temp=0.

        Returns:
            trainExamples: a list of examples of the form (state,pi,v)
                           pi is the MCTS informed policy vector, v is +1 if
                           the player eventually won the game, else -1.
        """
        logger = logging.getLogger("fireplace")
        logger.setLevel(logging.WARNING)
        trainExamples = []
        #print(id(self.game))
        #game = copy.deepcopy(self.game)
        current_game = self.game.getInitGame()
        #self.mcts = MCTS(self.game, self.nnet, self.args)   # reset search tree -> not needed since we get a copy of self.mcts in this child process, and MCTS itself uses a deep clone of this copy
        #curPlayer = 1 if f'{current_game.current_player}' == 'Player1' else -1
        #self.curPlayer = 1 if f'{current_game.current_player}' == 'Player1' else -1
        self.curPlayer = 1 if current_game.current_player.name == 'Player1' else -1
        #print(id(self.curPlayer))
        episodeStep = 0
        # timing
        # start = time.time()

        while True:
            episodeStep += 1
            # print('---Episode step ' + str(episodeStep) + '--- ' + current_process().name) #os.getpid())
            # print('TIME TAKEN : {0:03f}'.format(time.time()-start))
            # start = time.time()
            state = self.game.getState(current_game)                    # state is from the current player's perspective
            temp = int(episodeStep < self.args.tempThreshold)

            # TODO: No MCTS reset? Should work because player switch leads to new mirror state and info in tree could possibly be reused. Otherwise starting a new tree could speed things up here (faster lookups through smaller mcts lists)!
            pi = self.mcts.getActionProb(state, temp=temp)              # pi is from the current player's perspective
            # print(self.mcts)
            pi_reshape = np.reshape(pi, (21, 18))
            # sym = self.game.getSymmetries(state, pi)
            trainExamples.append([state, self.curPlayer, pi, None])     # TODO: Is None still needed?
            # for b,p in sym:
            #     trainExamples.append([b, self.curPlayer, p, None])
            action = np.random.choice(len(pi), p=pi)
            a, b = np.unravel_index(np.ravel(action, np.asarray(pi).shape), pi_reshape.shape)
            next_state, self.curPlayer = self.game.getNextState(self.curPlayer, (a[0], b[0]), current_game)

            r = self.game.getGameEnded(current_game, self.curPlayer)    # returns 0 if game has not ended, 1 if curPlayer won, -1 if curPlayer lost

            if r!=0:
                return [(x[0],x[2],r*((-1)**(x[1]!=self.curPlayer))) for x in trainExamples]

    def learn(self):
        """
        Performs numIters iterations with numEps episodes of self-play in each
        iteration. After every iteration, it retrains neural network with
        examples in trainExamples (which has a maximium length of maxlenofQueue).
        It then pits the new neural network against the old one and accepts it
        only if it wins >= updateThreshold fraction of games.
        """

        for i in range(1, self.args.numIters+1):
            # bookkeeping
            print('------ITER ' + str(i) + '------')

            # Find and load newest model (name scheme 'x.pth.tar' with highest x)
            modelfile = self.get_newest_model()
            print("Loading newest model:", modelfile)
            nnet.load_checkpoint(args.modelspath, modelfile)

            # examples of the iteration
            if not self.skipFirstSelfPlay or i>1:
                iterationTrainExamples = deque([], maxlen=self.args.maxlenOfQueue)

                # with ProcessPoolExecutor(self.args.numThreads) as executor:
                #     results = list(tqdm(executor.map(self.executeEpisode, range(self.args.numEps)), total=self.args.numEps, desc='Self-play matches'))
                # iterationTrainExamples = [r for r in results]

                # with Pool(self.args.numThreads) as pool:
                #     for result in list(tqdm(pool.imap(self.executeEpisode, range(self.args.numEps)), total=self.args.numEps, desc='Self-play matches')):
                #         iterationTrainExamples += result

                for result in parallel_process(self.executeEpisode, range(self.args.numEps), workers=self.args.numThreads, desc='Self-play matches'):
                    iterationTrainExamples += result

                # save the iteration examples to the history
                self.trainExamplesHistory.append(iterationTrainExamples)

            # if len(self.trainExamplesHistory) > self.args.numItersForTrainExamplesHistory:
            #     print("len(trainExamplesHistory) =", len(self.trainExamplesHistory), " => remove the oldest trainExamples")
            #     self.trainExamplesHistory.pop(0)
            # backup history to a file
            # NB! the examples were collected using the model from the previous iteration, so (i-1)
            self.saveTrainExamples(modelfile, i-1)

    def getCheckpointFile(self, modelfile, iteration):
        return modelfile + '_' + str(iteration)

    def saveTrainExamples(self, modelfile, iteration):
        folder = self.args.examplespath
        if not os.path.exists(folder):
            os.makedirs(folder)
        filename = os.path.join(folder, self.getCheckpointFile(modelfile, iteration) + ".examples")
        with open(filename, "wb+") as f:
            Pickler(f, protocol=pickle.HIGHEST_PROTOCOL).dump(self.trainExamplesHistory)
        f.closed

    # def loadTrainExamples(self):
    #     modelFile = os.path.join(self.args.load_folder_file[0], self.args.load_folder_file[1])
    #     examplesFile = modelFile+".examples"
    #     if not os.path.isfile(examplesFile):
    #         print(examplesFile)
    #         r = input("File with trainExamples not found. Continue? [y|n]")
    #         if r != "y":
    #             sys.exit()
    #     else:
    #         print("File with trainExamples found. Read it.")
    #         with open(examplesFile, "rb") as f:
    #             self.trainExamplesHistory = Unpickler(f).load()
    #         f.closed
    #         # examples based on the model were already collected (loaded)
    #         self.skipFirstSelfPlay = True

    def list_files(self, directory, extension):
        return (f for f in os.listdir(directory) if f.endswith('.' + extension))

    def get_modelnumber(self, filename):
        file_name = os.path.basename(filename)
        index_of_dot = file_name.index('.')
        file_name_without_extension = file_name[:index_of_dot]
        return int(file_name_without_extension)

    def get_newest_model(self):
        files = self.list_files(args.modelspath, "pth.tar")
        modelnumbers = list(map(self.get_modelnumber, files))
        maximum = max(modelnumbers)
        return str(maximum) + ".pth.tar"

if __name__=="__main__":
    #freeze_support()
    # Start processes with lower priority to prevent system overload/hangs/freezes. Also set multiprocessing start method to spawn for Linux, since forking makes trouble
    p = psutil.Process(os.getpid())
    if sys.platform.startswith('win32'):
        p.nice(psutil.BELOW_NORMAL_PRIORITY_CLASS)
    elif sys.platform.startswith('linux'):
        p.nice(5)
        mp.set_start_method('spawn')

    # Set number of threads for OpenMP (CPU)
    os.environ["OMP_NUM_THREADS"] = "1"

    g = Game(is_basic=True)
    # Suppress logging from fireplace
    logger = logging.getLogger("fireplace")
    logger.setLevel(logging.WARNING)

    nnet = nn()
    nnet.save_checkpoint(folder=args.modelspath, filename='0.pth.tar')

    c = Coach(g, nnet, args)
    c.learn()


import keras
import numpy as np
import librosa
import time
import sys
from WavFileHelper import WavFileHelper

MODEL_PATH = "currentModel.h5"
NUM_SAMPLES_TO_CONCIDER = 44100

class _EmotionSpottingService:
    model = None
    _mappings = ["nothappy", "happy", "interested"]
    _instance = None

    def predict(self, filePath):
        helper = WavFileHelper()
        MFCCs = helper.extract_mfcc(filePath)
        if len(MFCCs) == 0:
            print('Audio snippet is too short!')
            return 'FAILURE'
        emotionPredictions = []
        seconds = 0
        for mfcc in MFCCs:
            reshapedMfcc = mfcc[np.newaxis, ..., np.newaxis]
            predictions = self.model.predict(reshapedMfcc)
            predictedIndex = np.argmax(predictions)
            predictedKeyword = self._mappings[predictedIndex]
            timeCode = time.strftime('%H:%M:%S', time.gmtime(seconds))
            emotionPredictions.append(
                {"timeCode": timeCode, "emotion": predictedKeyword})
            seconds += 2
        return emotionPredictions

def EmotionSpottingService():
    if _EmotionSpottingService._instance == None:
        _EmotionSpottingService._instance = _EmotionSpottingService()
        _EmotionSpottingService.model = keras.models.load_model(MODEL_PATH)
    return _EmotionSpottingService._instance


#! /usr/bin/python
import math
import os
import string
import struct
import sys
import time
import csv
import numpy

#functions including
#1. simple classes to store all the information about each species and reaction.
#2. Functions to read in the species and reaction file and check for sanity
#3. Functions to write out files necessary for UCLCHEM

##########################################################################################
#1. simple classes to store all the information about each species and reaction.
#largely just to make the other functions more readable.
##########################################################################################
class Species:
	def __init__(self, inputRow):
		self.name = inputRow[0]
		self.mass = inputRow[1]
		self.bindener = float(inputRow[2])
		self.solidFraction = float(inputRow[3])
		self.monoFraction = float(inputRow[4])
		self.volcFraction = float(inputRow[5])
		self.enthalpy = float(inputRow[6])

	def is_grain_species(self):
		return self.name[0] == '#'

	def is_ion(self):
		return (self.name[-1] == "+" or self.name[-1] == "-")

class Reaction:
	def __init__(self, inputRow):
		self.reactants = [inputRow[0], inputRow[1], self.NANCheck(inputRow[2])]
		self.products = [inputRow[3], self.NANCheck(
			inputRow[4]), self.NANCheck(inputRow[5]), self.NANCheck(inputRow[6])]
		self.alpha = float(inputRow[7])
		self.beta = float(inputRow[8])
		self.gamma = float(inputRow[9])
		self.templow = inputRow[10]
		self.temphigh = inputRow[11]

	def NANCheck(self, a):
		aa = a if a else 'NAN'
		return aa

##########################################################################################
#2. Functions to read in the species and reaction file and check for sanity
##########################################################################################

# Read the entries in the specified species file
def read_species_file(fileName):
	speciesList = []
	f = open(fileName, 'rb')
	reader = csv.reader(f, delimiter=',', quotechar='|')
	for row in reader:
		if row[0] != "NAME" and "!" not in row[0]:
			speciesList.append(Species(row))
	nSpecies = len(speciesList)
	return nSpecies, speciesList

# Read the entries in the specified reaction file and keep the reactions that involve the species in our species list

def read_reaction_file(fileName, speciesList, ftype):
	reactions = []
	keepList = []
	# keeplist includes the elements that ALL the reactions should be formed from
	keepList.extend(['', 'NAN', '#', 'E-', 'e-', 'ELECTR', 'PHOTON', 'CRP', 'CRPHOT', 'FREEZE', 'CRH',
                  'PHOTD', 'THERM', 'XRAY', 'XRSEC', 'XRLYA', 'XRPHOT', 'DESOH2', 'DESCR', 'DEUVCR', "CHEMDES", "DIFF"])
	for species in speciesList:
		keepList.append(species.name)
	if ftype == 'UMIST':  # if it is a umist database file
		f = open(fileName, 'rb')
		reader = csv.reader(f, delimiter=':', quotechar='|')
		for row in reader:
			# if all the reaction elements belong to the keeplist
			if all(x in keepList for x in [row[2], row[3], row[4], row[5], row[6], row[7]]):
				#umist file doesn't have third reactant so add space and has a note for how reactions there are so remove that
				reactions.append(Reaction(row[2:4]+['']+row[4:8]+row[9:]))
	if ftype == 'UCL':  # if it is a ucl made (grain?) reaction file
		f = open(fileName, 'rb')
		reader = csv.reader(f, delimiter=',', quotechar='|')
		for row in reader:
			# if all the reaction elements belong to the keeplist
			if all(x in keepList for x in row[0:7]):
				row[10] = 0.0
				row[11] = 10000.0
				reactions.append(Reaction(row))

	nReactions = len(reactions)
	return nReactions, reactions

def remove_duplicate_species(speciesList):
		#check for duplicate species
	duplicates = 0
	duplicate_list = []
	for i in range(0, len(speciesList)):
		for j in range(0, len(speciesList)):
			if speciesList[i].name == speciesList[j].name:
				if (j != i) and speciesList[i].name not in duplicate_list:
					print "\t {0} appears twice in input species list".format(speciesList[i].name)
					duplicate_list.append(speciesList[i].name)

	for duplicate in duplicate_list:
		removed = False
		i = 0
		while not removed:
			if speciesList[i].name == duplicate:
				del speciesList[i]
				print "\tOne entry of {0} removed from list".format(duplicate)
				removed = True
			else:
				i += 1
	return speciesList

#Look for possibly incorrect parts of species list

def filter_species(speciesList, reactionList):
	#check for species not involved in any reactions
	lostSpecies = []
	for species in speciesList:
		keepFlag = False
		for reaction in reactionList:
			if species.name in reaction.reactants or species.name in reaction.products:
				keepFlag = True
		if not keepFlag:
			lostSpecies.append(species.name)
			speciesList.remove(species)

	print '\tSpecies in input list that do not appear in final list:'
	print '\t', lostSpecies
	print '\n'
	return speciesList

#All species should freeze out at least as themselves and all grain species should desorb according to their binding energy
#This function adds those reactions automatically to slim down the grain file

def add_desorb_reactions(speciesList, reactionList):
	for species in speciesList:
		if species.is_grain_species():
			for reacType in ['DESOH2', "DESCR", "DEUVCR"]:
				newReaction = Reaction([species.name, reacType, 'NAN', species.name[1:],
                                    'NAN', 'NAN', 'NAN', 1, 0, species.bindener, 0.0, 10000.0])
				reactionList.append(newReaction)
	return reactionList

#check reactions to alert user of potential issues including repeat reactions
#and multiple freeze out routes

def reaction_check(speciesList, reactionList, freeze_check=True):

	#first check for multiple freeze outs so user knows to do alphas
	print "\tSpecies with multiple freeze outs, check alphas:"
	for spec in speciesList:
		freezes = 0
		for reaction in reactionList:
			if (spec.name in reaction.reactants and 'FREEZE' in reaction.reactants):
				freezes += 1
		if (freezes > 1):
			print "\t{0} freezes out through {1} routes".format(spec.name, freezes)
		if freezes < 1 and not spec.is_grain_species() and freeze_check:
			print "\t{0} does not freeze out".format(spec.name, freezes)
	#now check for duplicate reactions
	duplicate_list = []
	print "\n\tPossible duplicate reactions for manual removal:"
	duplicates = 0
	for i, reaction1 in enumerate(reactionList):
		if i not in duplicate_list:
			for j, reaction2 in enumerate(reactionList):
				if i != j:
					if set(reaction1.reactants) == set(reaction2.reactants):
						if set(reaction1.products) == set(reaction2.products):
							print "\tReactions {0} and {1} are possible duplicates".format(i+1, j+1)
							print "\t", str(i+1), reaction1.reactants, "-->", reaction1.products
							print "\t", str(j+1), reaction1.reactants, "-->", reaction2.products
							duplicates += 1
							duplicate_list.append(i)
							duplicate_list.append(j)

	if (duplicates == 0):
		print "\tNone"

#capitalize files

def make_capitals(fileName):
	a = open(fileName).read()
	output = open(fileName, mode='w')
	output.write(a.upper())
	output.close()

def find_constituents(speciesList):
	elementList = ['H', 'D', 'HE', 'C', 'N', 'O', 'F',
                'P', 'S', 'CL', 'LI', 'NA', 'MG', 'SI', 'PAH', '15N']
	elementMass = [1, 2, 4, 12, 14, 16, 19, 31, 32, 35, 3, 23, 24, 28, 420, 15]
	symbols = ['#', '+', '-', '(', ')']

	for species in speciesList:
		speciesName = species.name
		i = 0
		atoms = []
		bracket = False
		bracketContent = []
		#loop over characters in species name to work out what it is made of
		while i < len(speciesName):
			#if character isn't a #,+ or - then check it otherwise move on
			if speciesName[i] not in symbols:
				if i+1 < len(speciesName):
					#if next two characters are (eg) 'MG' then atom is Mg not M and G
					if speciesName[i:i+3] in elementList:
						j = i+3
					elif speciesName[i:i+2] in elementList:
						j = i+2
					#otherwise work out which element it is
					elif speciesName[i] in elementList:
						j = i+1

				#if there aren't two characters left just try next one
				elif speciesName[i] in elementList:
					j = i+1
				#if we've found a new element check for numbers otherwise print error
				if j > i:
					if bracket:
						bracketContent.append(speciesName[i:j])
					else:
						atoms.append(speciesName[i:j])  # add element to list
					if j < len(speciesName):
						if is_number(speciesName[j]):
							if int(speciesName[j]) > 1:
								for k in range(1, int(speciesName[j])):
									if bracket:
										bracketContent.append(speciesName[i:j])
									else:
										atoms.append(speciesName[i:j])
								i = j+1
							else:
								i = j
						else:
							i = j
					else:
						i = j
				else:
					print speciesName[i]
					print"\t{0} contains elements not in element list:".format(speciesName)
					print elementList
			else:
				#if symbol is start of a bracketed part of molecule, keep track
				if (speciesName[i] == "("):
					bracket = True
					bracketContent = []
					i += 1
				#if it's the end then add bracket contents to list
				elif speciesName[i] == ")":
					if is_number(speciesName[i+1]):
						for k in range(0, int(speciesName[i+1])):
							atoms.extend(bracketContent)
						i += 2
					else:
						atoms.extend(bracketContent)
						i += 1
				#otherwise move on
				else:
					i += 1

		species.n_atoms = len(atoms)
		mass = 0
		for atom in atoms:
			mass += elementMass[elementList.index(atom)]
		if mass != float(species.mass):
			#print "\tcalculated mass of {0} does not match input mass".format(speciesName)
			#print "\tcalculated mass: {0} \t input mass: {1}\n".format(mass,species.mass)
			#print "\tkeeping calculated mass\n"
			species.mass = str(mass)
	return speciesList

def is_number(s):
    try:
        float(s)
        return True
    except ValueError:
        return False
##########################################################################################
#3. Functions to write out files necessary for UCLCHEM
##########################################################################################

# Write the species file in the desired format

def write_species(fileName, speciesList):
	f = open(fileName, 'wb')
	writer = csv.writer(f, delimiter=',', quotechar='|',
	                    quoting=csv.QUOTE_MINIMAL, lineterminator='\n')
	nSpecies = len(speciesList)
	for species in speciesList:
		writer.writerow([species.name, species.mass, species.n_atoms])

# Write the reaction file in the desired format

def write_reactions(fileName, reactionList):
	f = open(fileName, 'wb')
	writer = csv.writer(f, delimiter=',', quotechar='|',
	                    quoting=csv.QUOTE_MINIMAL, lineterminator='\n')
	nReactions = len(reactionList)
	for reaction in reactionList:
		#if statement changes beta for ion freeze out to 1. This is how ucl_chem recognises ions when calculating freeze out rate
		if ('FREEZE' in reaction.reactants and reaction.reactants[0][-1] == '+'):
			reaction.beta = 1
		writer.writerow(reaction.reactants+reaction.products +
		                [reaction.alpha, reaction.beta, reaction.gamma, reaction.templow, reaction.temphigh])

def write_odes_f90(fileName, speciesList, reactionList):
	output = open(fileName, mode='w')
	#go through every species and build two strings, one with eq for all destruction routes and one for all formation
	ydotString = build_ode_string(speciesList, reactionList)
	output.write(ydotString)
	output.close()

def build_ode_string(speciesList, reactionList):
	odeString = ""
	nSpecies = len(speciesList)
	nReactions = len(reactionList)

	for n, species in enumerate(speciesList):
		lossString = ''
		formString = ''
		#go through entire reaction list
		for i, reaction in enumerate(reactionList):

			#if species appear in reactants, reaction is a destruction route
			if species.name in reaction.reactants:
				bodyCount = 0  # two or more bodies in a reaction mean we multiply rate by density so need to keep track
				#easy for h2 formation
				if is_H2_formation(reaction.reactants, reaction.products):
					lossString += '-2*RATE('+str(i+1)+')*D'
					continue
				#multiply string by number of time species appears in reaction. multiple() defined below
				#so far reaction string is rate(reaction_index) indexs are all +1 for fortran array indexing
				lossString += '-' + \
					multiple(reaction.reactants.count(species.name))+'RATE('+str(i+1)+')'

				#now add *Y(species_index) to string for every reactant
				for reactant in set(reaction.reactants):
					n_appearances = reaction.reactants.count(reactant)
					#every species appears at least once in its loss reaction
					#so we multiply entire loss string by Y(species_index) at end
					#thus need one less Y(species_index) per reaction
					if reactant == species.name:
						bodyCount += n_appearances
						if n_appearances > 1:
							for j, possibleReactants in enumerate(speciesList):
								if reactant == possibleReactants.name:
									for appearance in range(1, n_appearances):
										lossString += '*Y('+str(j+1)+')'
									continue
					else:
						#look through species list and find reactant
						for j, possibleReactants in enumerate(speciesList):
							if reactant == possibleReactants.name:
								for appearance in range(n_appearances):
									lossString += '*Y('+str(j+1)+')'
									bodyCount += 1
								continue
				#now string is rate(reac_index)*Y(species_index1)*Y(species_index2) may need *D if total rate is
				#proportional to density
				if reaction.reactants.count('FREEZE') > 0 or reaction.reactants.count('DESOH2') > 0:
					lossString += '*D'
				for body in range(1, bodyCount):
					lossString += "*D"

			#same process as above but rate is positive for reactions where species is positive
			if species.name in reaction.products:
				bodyCount = 0  # two or more bodies in a reaction mean we multiply rate by density so need to keep track

				if is_H2_formation(reaction.reactants, reaction.products):
					#honestly H should be index 1 but lets check
					H_index = speciesList.index(
						next((x for x in speciesList if x.name == 'H')))
					formString += '+RATE('+str(i+1)+')*Y('+str(H_index+1)+')*D'
					continue

				#multiply string by number of time species appears in reaction. multiple() defined below
				#so far reaction string is rate(reaction_index) indexs are all +1 for fortran array indexing
				formString += '+' + \
					multiple(reaction.products.count(species.name))+'RATE('+str(i+1)+')'

				#now add *Y(species_index) to string for every reactant
				for reactant in set(reaction.reactants):
					n_appearances = reaction.reactants.count(reactant)
					for j, possibleReactants in enumerate(speciesList):
						if reactant == possibleReactants.name:
							for appearance in range(n_appearances):
								formString += '*Y('+str(j+1)+')'
								bodyCount += 1
							continue

				#now string is rate(reac_index)*Y(species_index1)*Y(species_index2) may need *D if total rate is
				#proportional to density
				if reaction.reactants.count('FREEZE') > 0 or reaction.reactants.count('DESOH2') > 0:
					formString += '*D'
				for body in range(1, bodyCount):
					formString += "*D"

		if lossString != '':
			lossString = '      LOSS = '+lossString+'\n'
			lossString = truncate_line(lossString)
			odeString += lossString
		if formString != '':
			formString = '      PROD = '+formString+'\n'
			formString = truncate_line(formString)
			odeString += formString

		#start with empty string and add production and loss terms if they exists
		ydotString = ''
		if formString != '':
			ydotString += 'PROD'
			if lossString != '':
				ydotString += '+'
		if lossString != '':
			ydotString += 'Y('+str(n+1)+')*LOSS'

		#if we have prod and/or loss add ydotstring to odes
		if ydotString != '':
			ydotString = '      YDOT('+str(n+1)+') = '+ydotString+"\n"
			ydotString = truncate_line(ydotString)
			odeString += ydotString
		else:
			ydotString = '      YDOT('+str(n+1)+') = 0.0\n'
			ydotString = truncate_line(ydotString)
			odeString += ydotString
	return odeString

#create a file containing length of each list of moleculetypes and then the two lists (gas and grain) of species in each type
#as  well as fraction that evaporated in each type of event

def write_evap_lists(openFile, speciesList):
	grainlist = []
	mgrainlist = []
	solidList = []
	monoList = []
	volcList = []
	bindEnergyList = []
	enthalpyList = []

	for i, species in enumerate(speciesList):
		if species.name[0] == '#':
			#find gas phase version of grain species. For #CO it looks for first species in list with just CO and then finds the index of that
			try:
				j = speciesList.index(
					next((x for x in speciesList if x.name == species.name[1:])))
			except:
				print "\n**************************************\nWARNING\n**************************************"
				print "{0} has no gas phase equivalent in network. Every species should at least freeze out and desorb.".format(species.name)
				print "ensure {0} is in the species list, and at least one reaction involving it exists and try again".format(species.name[1:])
				print "Alternatively, provide the name of the gas phase species you would like {0} to evaporate as".format(species.name)
				input = raw_input(
					"type x to quit Makerates or any species name to continue\n")
				if input.lower() == "x":
					exit()
				else:
					j = speciesList.index(
						next((x for x in speciesList if x.name == input.upper())))

			#plus ones as fortran and python label arrays differently
			mgrainlist.append(i+1)
			grainlist.append(j+1)
			solidList.append(species.solidFraction)
			monoList.append(species.monoFraction)
			volcList.append(species.volcFraction)
			bindEnergyList.append(species.bindener)
			enthalpyList.append(species.enthalpy)

	openFile.write(array_to_string("gasGrainList", grainlist, type="int"))
	openFile.write(array_to_string("grainList", mgrainlist, type="int"))
	openFile.write(array_to_string("solidFractions", solidList, type="float"))
	openFile.write(array_to_string("monoFractions", monoList, type="float"))
	openFile.write(array_to_string("volcanicFractions", volcList, type="float"))
	openFile.write(array_to_string(
		"bindingEnergy", bindEnergyList, type="float", parameter=False))
	openFile.write(array_to_string(
		"formationEnthalpy", enthalpyList, type="float"))

	return len(grainlist)

# Create the appropriate multiplication string for a given number

def multiple(number):
    if number == 1:
    	return ''
    else:
    	return str(number)+'*'

def truncate_line(input, codeFormat='F90', continuationCode=None):
	lineLength = 72
	maxlines = 300
	lines = 0
	result = ''
	i = 0
	j = 0
	while i+j < len(input):
		j += 1
		if j > lineLength:
			#important not to break entries so split lines at ,s
			try:
				k = input[i+j-16:i+j].index(",")
			except:
				try:
					k = input[i+j-16:i+j].index("*")
				except:
					k = input[i+j-16:i+j].index(")")
			j = j-16+k
			result += input[i:i+j]+"&\n    &"
			i = i+j
			j = 0
	result += input[i:i+j]
	return result

def is_H2_formation(reactants, products):
    nReactants = len([species for species in reactants if species != ''])
    nProducts = len([species for species in products if species != ''])
    if nReactants == 2 and nProducts == 1:
        if reactants[0] == 'H' and reactants[1] == 'H' and products[0] == 'H2':
        	return True
    if nReactants == 3 and nProducts == 2:
        if reactants[0] == 'H' and reactants[1] == 'H' and reactants[2] == '#' and products[0] == 'H2' and products[1] == '#':
        	return True
    return False

def write_network_file(fileName, speciesList, reactionList):
	openFile = open(fileName, "wb")
	openFile.write("integer, parameter :: nSpec={0}, nReac={1}\n".format(
		len(speciesList), len(reactionList)))

	#write arrays of all species stuff
	names = []
	atoms = []
	masses = []
	for species in speciesList:
		names.append(species.name)
		masses.append(float(species.mass))
		atoms.append(species.n_atoms)
	openFile.write(array_to_string("specname", names, type="string"))
	openFile.write(array_to_string("mass", masses, type="float"))
	openFile.write(array_to_string("atomCounts", atoms, type="int"))

	#then write evaporation stuff
	nGrain = write_evap_lists(openFile, speciesList)

	#finally all reactions
	reactant1 = []
	reactant2 = []
	reactant3 = []
	prod1 = []
	prod2 = []
	prod3 = []
	prod4 = []
	alpha = []
	beta = []
	gama = []
	for reaction in reactionList:
		reactant1.append(reaction.reactants[0])
		reactant2.append(reaction.reactants[1])
		reactant3.append(reaction.reactants[2])
		prod1.append(reaction.products[0])
		prod2.append(reaction.products[1])
		prod3.append(reaction.products[2])
		prod4.append(reaction.products[3])
		alpha.append(reaction.alpha)
		beta.append(reaction.beta)
		gama.append(reaction.gamma)
	openFile.write(array_to_string("re1", reactant1, type="string"))
	openFile.write(array_to_string("re2", reactant2, type="string"))
	openFile.write(array_to_string("re3", reactant3, type="string"))
	openFile.write(array_to_string("p1", prod1, type="string"))
	openFile.write(array_to_string("p2", prod2, type="string"))
	openFile.write(array_to_string("p3", prod3, type="string"))
	openFile.write(array_to_string("p4", prod4, type="string"))
	openFile.write(array_to_string("alpha", alpha, type="float", parameter=False))
	openFile.write(array_to_string("beta", beta, type="float", parameter=False))
	openFile.write(array_to_string("gama", gama, type="float", parameter=False))

def array_to_string(name, array, type="int", parameter=True):
	if parameter:
		outString = ", parameter :: "+name+" ({0})=(/".format(len(array))
	else:
		outString = " :: "+name+" ({0})=(/".format(len(array))
	if type == "int":
		outString = "integer"+outString
		for value in array:
			outString += "{0},".format(value)
	elif type == "float":
		outString = "double precision"+outString
		for value in array:
			outString += "{0:.4e},".format(value)
	elif type == "string":
		outString = "character(Len=10)"+outString
		for value in array:
			outString += "\""+value.ljust(10)+"\","
	else:
		print "Not a valid type for array to string"
	outString = outString[:-1]+"/)\n"
	outString = truncate_line(outString)
	return outString


from model import generate_model
import tensorflow as tf
import os
import numpy as np

import time
from random import randint
from nes_py.wrappers import JoypadSpace
import gym_super_mario_bros
from gym_super_mario_bros.actions import SIMPLE_MOVEMENT

from auto_everything.base import IO
io = IO()

env = gym_super_mario_bros.make('SuperMarioBros-v2')
env = JoypadSpace(env,  SIMPLE_MOVEMENT)

model_file_path = './nn_model.HDF5'
final_model_file_path = './final_nn_model.HDF5'
if os.path.exists(model_file_path):
    model = tf.keras.models.load_model(model_file_path)
else:
    model = generate_model()

# env.action_space.sample() = numbers, for example, 0,1,2,3...
# state = RGB of raw picture; is a numpy array with shape (240, 256, 3)
# reward = int; for example, 0, 1 ,2, ...
# done = False or True
# info = {'coins': 0, 'flag_get': False, 'life': 3, 'score': 0, 'stage': 1, 'status': 'small', 'time': 400, 'world': 1, 'x_pos': 40}

identity = np.identity(env.action_space.n)  # for quickly get a hot vector, like 0001000000000000

state = None
reward = 0
done = True
info = None

last_state = None
last_info = None

max_x_pos = io.read_settings("max_x_pos", 0)
history_x_pos_list_where_I_die = []
learning_area = 30
while 1:
    for step in range(1000):
        if done or reward < -5:
            state = env.reset()
            if last_info != None:
                if len(history_x_pos_list_where_I_die) > 0:
                    if last_info['x_pos'] < history_x_pos_list_where_I_die[0]:
                        history_x_pos_list_where_I_die.insert(0, last_info['x_pos'])
                else:
                    history_x_pos_list_where_I_die.insert(0, last_info['x_pos'])

        if randint(1, 10) == 1 or not isinstance(last_state, (np.ndarray, np.generic)):
            action = env.action_space.sample()
            print(f"                            |  random  |  ")
        else:
            action = np.argmax(model.predict(np.expand_dims(last_state, axis=0)))
            print(f"                            |  predict  |  ")
            print(f"x_pos: {info['x_pos']}  |  reward: {reward}")

        if isinstance(state, (np.ndarray, np.generic)) and info != None:
            last_state = state
            last_info = info
        state, reward, done, info = env.step(action)
        print(f"x_pos: {info['x_pos']}  |  reward: {reward}")
        if isinstance(last_state, (np.ndarray, np.generic)) and last_info != None:
            if ((reward > 0) and (len(history_x_pos_list_where_I_die) > 0) and (history_x_pos_list_where_I_die[0]-learning_area < last_info['x_pos'] < history_x_pos_list_where_I_die[0]+learning_area)):
                print(f"curent: {last_info['x_pos']}, historical: {history_x_pos_list_where_I_die[0]}")
                if last_info["x_pos"] > history_x_pos_list_where_I_die[0]:
                    history_x_pos_list_where_I_die.pop(0)
                    print("fucked")
                    time.sleep(1)

                model.train_on_batch(x=np.expand_dims(last_state, axis=0), y=identity[action: action+1])
                print(f"                                            training happend")

        env.render()

        # additional operation
        if info['x_pos'] > max_x_pos:
            max_x_pos = info['x_pos']
            io.write_settings("max_x_pos", int(max_x_pos))
            if info['life'] == 2:
                model.save(model_file_path)
                pass

        if info["stage"] == 2:
            model.save(final_model_file_path)
            input("congraduations!")
            exit()

env.close()


#!/usr/bin/env python3

# Copyright 2019 NVIDIA Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import os
import glob
import argparse
import PIL.Image
import numpy as np
import tensorrt as trt
import pycuda.driver as cuda
import pycuda.autoinit  # To automatically manage CUDA context creation and cleanup

def load_normalized_test_case(test_images, pagelocked_buffer, preprocess_func):
    # Expected input dimensions
    C, H, W = (3, 224, 224)
    # Normalize the images, concatenate them and copy to pagelocked memory.
    data = np.asarray([preprocess_func(PIL.Image.open(img), C, H, W) for img in test_images]).flatten()
    np.copyto(pagelocked_buffer, data)

class HostDeviceMem(object):
    r""" Simple helper data class that's a little nicer to use than a 2-tuple.
    """
    def __init__(self, host_mem, device_mem):
        self.host = host_mem
        self.device = device_mem

    def __str__(self):
        return "Host:\n" + str(self.host) + "\nDevice:\n" + str(self.device)

    def __repr__(self):
        return self.__str__()

def allocate_buffers(engine: trt.ICudaEngine, batch_size: int):
    print('Allocating buffers ...')

    inputs = []
    outputs = []
    dbindings = []

    stream = cuda.Stream()

    for binding in engine:
        size = batch_size * trt.volume(engine.get_binding_shape(binding))
        dtype = trt.nptype(engine.get_binding_dtype(binding))
        # Allocate host and device buffers
        host_mem = cuda.pagelocked_empty(size, dtype)
        device_mem = cuda.mem_alloc(host_mem.nbytes)
        # Append the device buffer to device bindings.
        dbindings.append(int(device_mem))

        # Append to the appropriate list.
        if engine.binding_is_input(binding):
            inputs.append(HostDeviceMem(host_mem, device_mem))
        else:
            outputs.append(HostDeviceMem(host_mem, device_mem))

    return inputs, outputs, dbindings, stream

def infer(engine_path, preprocess_func, batch_size=8, input_images=[], labels=[], num_classes=3):
    with open(engine_path, 'rb') as f, trt.Runtime(trt.Logger(trt.Logger.WARNING)) as runtime:
        engine = runtime.deserialize_cuda_engine(f.read())

        # Allocate buffers and create a CUDA stream.
        inputs, outputs, dbindings, stream = allocate_buffers(engine, batch_size)

        # Contexts are used to perform inference.
        with engine.create_execution_context() as context:
            test_images = np.random.choice(input_images, size=batch_size)
            load_normalized_test_case(test_images, inputs[0].host, preprocess_func)

            inp = inputs[0]
            # Transfer input data to the GPU.
            cuda.memcpy_htod(inp.device, inp.host)

            # Run inference.
            context.execute(batch_size, dbindings)

            out = outputs[0]
            # Transfer predictions back to host from GPU
            cuda.memcpy_dtoh(out.host, out.device)
            out_np = np.array(out.host)

            # Split 1-D output of length N*labels into 2-D array of (N, labels)
            batch_outs = np.array(np.split(out_np, batch_size))
            for test_image, batch_out in zip(test_images, batch_outs):
                topk_indices = np.argsort(batch_out)[-1*num_classes:][::-1]
                preds = labels[topk_indices]
                probs = batch_out[topk_indices]
                print("Input image:", test_image)
                for pred, prob in zip(preds, probs):
                    print("\tPrediction: {:30} Probability: {:0.2f}".format(pred, prob))

def get_inputs(filename=None, directory=None, allowed_extensions=(".jpeg", ".jpg", ".png")):
    filenames = []
    if filename:
        filenames.append(filename)
    if directory:
        dir_files = [path for path in glob.iglob(os.path.join(directory, "**"), recursive=True)
                     if os.path.isfile(path) and path.lower().endswith(allowed_extensions)]
        filenames.extend(dir_files)

    if len(filenames) <= 0:
        raise ValueError("ERROR: No valid inputs given.")

    return filenames

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description='Run inference on TensorRT engines for Imagenet-based Classification models.')
    parser.add_argument('--engine', type=str, required=True,
                        help='Path to TensorRT engine {resnet50, vgg16, inception_v1, mobilenetv2-1.0...}')
    parser.add_argument('-f', '--file', default=None, type=str,
                        help="Path to input image.")
    parser.add_argument('-d', '--directory', default=None, type=str,
                        help="Path to directory of input images.")
    parser.add_argument("-l", "--labels", type=str, default=os.path.join("labels", "imagenet1k_labels.txt"),
                        help="Path to file containing model prediction labels.")
    parser.add_argument('-n', '--num_classes', default=3, type=int,
                        help="Top-K predictions to output.")
    parser.add_argument('-b', '--batch_size', default=8, type=int,
                        help="Number of inputs to send in parallel (up to max batch size of engine).")
    parser.add_argument("-p", "--preprocess_func", type=str, default=None,
                        help="Name of function defined in 'processing.py' to use for pre-processing calibration data.")
    args = parser.parse_args()

    input_images = get_inputs(args.file, args.directory)
    with open(args.labels, "r") as f:
        labels = np.array(f.read().splitlines())

    # Choose pre-processing function for inference inputs
    import processing
    if args.preprocess_func is not None:
        preprocess_func = getattr(processing, args.preprocess_func)
    else:
        preprocess_func = processing.preprocess_imagenet

    infer(args.engine, preprocess_func, batch_size=args.batch_size, input_images=input_images,
          labels=labels, num_classes=args.num_classes)


#!/usr/bin/python
# Copyright (c) Facebook, Inc. and its affiliates.
# All rights reserved.
#
# This source code is licensed under the BSD-style license found in the
# LICENSE file in the root directory of this source tree.
#
# LASER  Language-Agnostic SEntence Representations
# is a toolkit to calculate multilingual sentence embeddings
# and to use them for document classification, bitext filtering
# and mining
#
# --------------------------------------------------------
#
# Python tool to search for paraphrases in FAISS index

import re
import sys
import os.path
import tempfile
import argparse
import faiss
import time
import pdb
import numpy as np
from collections import namedtuple

# get environment
assert os.environ.get('LASER'), 'Please set the enviornment variable LASER'
LASER = os.environ['LASER']

sys.path.append(LASER + '/source/lib')
from indexing import IndexLoad, IndexTextOpen, IndexTextQuery, SplitOpen, SplitAccess
from embed import SentenceEncoder, EncodeLoad, EncodeFile, EncodeTime
from text_processing import Token, BPEfastApply

SPACE_NORMALIZER = re.compile("\s+")
Batch = namedtuple('Batch', 'srcs tokens lengths')

# calculate L2 distance between [x]
# and the vectors referenced in idxs
# x should be already normalized
def IndexDistL2(X, E, D, I, thresh=1.0, dtype=np.float32, sort=True):
    nb, nK = I.shape
    dim = X.shape[1]
    dist_l2 = np.empty((nb, nK), dtype=np.float32)
    y = np.empty((1, dim), dtype=dtype)
    for i in range(nb):
        for k in range(nK):
            if D[i, k] <= thresh:
                # get embedding from disk
                np.copyto(y, SplitAccess(E, I[i, k]))
                faiss.normalize_L2(y)
                dist_l2[i, k] = 1.0 - np.dot(X[i], y[0])
            else:
                # exclude sentences which already have a huge FAISS distance
                # (getting embeddings from disk is very time consumming)
                dist_l2[i, k] = 1.0

        if sort:
            # re-sort according to L2
            idxs = np.argsort(dist_l2[i], axis=0)
            dist_l2[i] = dist_l2[i][idxs]
            I[i] = I[i][idxs]

    return dist_l2, I

###############################################################################
#
# Apply an absolute threshold on the distance
#
###############################################################################

def MarginAbs(em, ofp, params, args, stats):
    D, I = params.idx.search(em, args.kmax)
    thresh = args.threshold_faiss
    if args.embed:
        D, I = IndexDistL2(em, params.E, D, I, args.threshold_faiss)
        thresh = args.threshold_L2

    for n in range(D.shape[0]):

        prev = {}  # for deduplication
        for i in range(args.kmax):
            txt = IndexTextQuery(params.T, params.R, I[n, i])
            if (args.dedup and txt not in prev) and D[n, i] <= thresh:
                prev[txt] = 1
                ofp.write('{:d}\t{:7.5f}\t{}\n'
                          .format(stats.nbs, D[n, i], txt))
                stats.nbp += 1

        # display source sentece if requested
        if (args.include_source == 'matches' and len(prev) > 0):
            ofp.write('{:d}\t{:6.1f}\t{}\n'
                      .format(stats.nbs, 0.0, sentences[n].replace('@@ ', '')))

        if args.include_source == 'always':
            ofp.write('{:d}\t{:6.1f}\t{}\n'
                      .format(stats.nbs, 0.0, sentences[n].replace('@@ ', '')))
        stats.nbs += 1

###############################################################################
#
# Apply an threshold on the ratio between distance and average
#
###############################################################################

def MarginRatio(em, ofp, params, args, stats):
    D, I = params.idx.search(em, args.margin_k)
    thresh = args.threshold
    if args.embed:
        D, I = IndexDistL2(em, params.E, D, I, args.threshold_faiss)
        thresh = args.threshold_L2

    Mean = D.mean(axis=1)
    for n in range(D.shape[0]):
        if D[n, 0] / Mean[n] <= args.threshold:
            if args.include_source == 'matches':
                ofp.write('{:d}\t{:6.1f}\t{}\n'
                          .format(stats.nbs, 0.0, sentences[n].replace('@@ ', '')))
            txt = IndexTextQuery(params.T, params.R, I[n, 0])
            ofp.write('{:d}\t{:7.5f}\t{}\n'.format(stats.nbs, D[n, 0], txt))
            stats.nbp += 1

        stats.nbs += 1

    if args.include_source == 'always':
        ofp.write('{:d}\t{:6.1f}\t{}\n'
                  .format(stats.nbs, 0.0, sentences[n].replace('@@ ', '')))

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

def MarginDist(em, ofp, params, args, stats):
    print('ERROR: MarginAbs not implemented')
    sys.exit(1)

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

def buffered_read(fp, buffer_size):
    buffer = []
    for src_str in fp:
        buffer.append(src_str.strip())
        if len(buffer) >= buffer_size:
            yield buffer
            buffer = []

    if len(buffer) > 0:
        yield buffer

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

parser = argparse.ArgumentParser('LASER: paraphrase tool')

parser.add_argument('--encoder', type=str, required=True,
    help='encoder to be used')
parser.add_argument('--encoding', default='utf-8',
    help='Character encoding for input/output')
parser.add_argument('--token-lang', type=str, default='--',
    help="Language of tokenizer ('--' for no tokenization)")
parser.add_argument('--bpe-codes', type=str, default=None, required=True,
    help='BPE codes')
parser.add_argument('--buffer-size', type=int, default=100,
    help='Buffer size (sentences)')
parser.add_argument('--max-tokens', type=int, default=12000,
    help='Maximum number of tokens to process in a batch')
parser.add_argument('--max-sentences', type=int, default=None,
    help='Maximum number of sentences to process in a batch')
parser.add_argument('--cpu', action='store_true',
    help='Use CPU instead of GPU')

parser.add_argument('--index', type=str, required=True,
    help='FAISS index')
parser.add_argument('--nprobe', type=int, default=128,
    help='FAISS: value of nprobe')
parser.add_argument('--text', type=str, required=True,
    help='File with indexed texts')
parser.add_argument(
    '--dim', type=int, default=1024,
    help='Dimension of specified sentence embeddings')
parser.add_argument(
    '--embed', type=str, default=None,
    help='Sentence embeddings, true L2 distance will be calculated when specified')

parser.add_argument('-i', '--input', type=str, required=True,
    help='Input text file')
parser.add_argument('-p', '--output', type=str, default='--',
    help='Output paraphrases')
parser.add_argument('--kmax', type=int, default=10,
    help='Max value of distance or margin of each paraphrase')
parser.add_argument('--dedup', type=int, default=1,
    help='Deduplicate list of paraphrases')
parser.add_argument('--include-source', default='never',
    choices=['never', 'matches', 'always'],
    help='Include source sentence in the list of paraphrases')
parser.add_argument('--margin',
    choices=['absolute', 'distance', 'ratio'],
    default='ratio', help='Margin function')
parser.add_argument('-T', '--threshold-margin', type=float, default=0.9,
    help='Threshold on margin')
parser.add_argument('--threshold-faiss', type=float, default=0.4,
    help='Threshold on FAISS distance')
parser.add_argument('--threshold-L2', type=float, default=0.2,
    help='Threshold on L2 distance')
parser.add_argument('--margin-k', type=int, default=4,
    help='Number of nearest neighbors for margin calculation')

parser.add_argument('--verbose', action='store_true',
    help='Detailed output')

print('\nLASER: paraphrase tool')
args = parser.parse_args()

# index,
# memory mapped texts, references and word counts
# encoder
params = namedtuple('params', 'idx T R W M E enc')

# open text and reference file
params.T, params.R, params.W, params.M = IndexTextOpen(args.text)

# Open on-disk embeddings for L2 distances
if args.embed:
    params.E = SplitOpen(args.embed, ['en'],
                                args.dim, np.float32, verbose=False)

# load FAISS index
params.idx = IndexLoad(args.index, args.nprobe)

# load sentence encoder
params.enc = EncodeLoad(args)

margin_methods = {'absolute': MarginAbs,
                  'distance': MarginDist,
                  'ratio': MarginRatio}

with tempfile.TemporaryDirectory() as tmpdir:
    ifile = args.input
    if args.token_lang != '--':
        ifile = os.path.join(tmpdir, 'tok')
        Token(args.input,
              ifile,
              lang=args.token_lang,
              romanize=True if args.token_lang == 'el' else False,
              lower_case=True, gzip=False,
              verbose=args.verbose, over_write=False)

    if args.bpe_codes:
        bpe_file = os.path.join(tmpdir, 'bpe')
        BPEfastApply(ifile,
                     bpe_file,
                     args.bpe_codes,
                     verbose=args.verbose, over_write=False)
        ifile = bpe_file

    print(' - processing (batch size is {:d})'.format(args.buffer_size))
    ifp = open(ifile, 'r', encoding=args.encoding, errors='surrogateescape')
    if args.output == '--':
        ofp = sys.stdout
    else:
        ofp = open(args.output, 'w', encoding=args.encoding, errors='surrogateescape')
    stats = namedtuple('stats', 'ns np')
    stats.nbs = 0
    stats.nbp = 0
    t = time.time()
    for sentences in buffered_read(ifp, args.buffer_size):
        embed = params.enc.encode_sentences(sentences)
        faiss.normalize_L2(embed)
        # call function for selected margin method
        margin_methods.get(args.margin)(embed, ofp, params, args, stats)
        if stats.nbs % 1000 == 0:
            print('\r - {:d} sentences {:d} paraphrases'
                  .format(stats.nbs, stats.nbp), end='')

    ifp.close()
    if args.output != '--':
        ofp.close()
    print('\r - {:d} sentences {:d} paraphrases'
          .format(stats.nbs, stats.nbp), end='')
    EncodeTime(t)


import torch as t
from torch import nn
from data import TripleDataLoader
from models.vgg import vgg16
from models.sketch_resnet import resnet34, resnet50
from utils.visualize import Visualizer
from torchnet.meter import AverageValueMeter
# import tqdm
from utils.extractor import Extractor
from sklearn.neighbors import NearestNeighbors
from torch.nn import DataParallel
from .TripletLoss import TripletLoss
from utils.test import Tester
import os
import numpy as np

class Config(object):
    def __init__(self):
        return

class TripletNet(object):

    def __init__(self, opt):
        # train config
        self.photo_root = opt.photo_root
        self.sketch_root = opt.sketch_root
        self.batch_size = opt.batch_size
        # self.device = opt.device
        self.epochs = opt.epochs
        self.lr = opt.lr

        # testing config
        self.photo_test = opt.photo_test
        self.sketch_test = opt.sketch_test
        self.test = opt.test
        self.test_f = opt.test_f

        self.save_model = opt.save_model
        self.save_dir = opt.save_dir

        # vis
        self.vis = opt.vis
        self.env = opt.env

        # fine_tune
        self.fine_tune = opt.fine_tune
        self.model_root = opt.model_root

        # dataloader config
        data_opt = Config()
        data_opt.photo_root = opt.photo_root
        data_opt.sketch_root = opt.sketch_root
        data_opt.batch_size = opt.batch_size

        self.dataloader_opt = data_opt

        # triplet config
        self.margin = opt.margin
        self.p = opt.p

        # feature extractor net
        self.net = opt.net
        self.cat = opt.cat

    def _get_vgg16(self, pretrained=True):
        model = vgg16(pretrained=pretrained)
        model.classifier[6] = nn.Linear(in_features=4096, out_features=125, bias=True)

        return model

    def _get_resnet34(self, pretrained=True):
        model = resnet34(pretrained=pretrained)
        model.fc = nn.Linear(in_features=512, out_features=125)

        return model

    def _get_resnet50(self, pretrained=True):
        model = resnet50(pretrained=pretrained)
        model.fc = nn.Linear(in_features=2048, out_features=125)

        return model

    def train(self):

        if self.net == 'vgg16':
            photo_net = DataParallel(self._get_vgg16()).cuda()
            sketch_net = DataParallel(self._get_vgg16()).cuda()
        elif self.net == 'resnet34':
            photo_net = DataParallel(self._get_resnet34()).cuda()
            sketch_net = DataParallel(self._get_resnet34()).cuda()
        elif self.net == 'resnet50':
            photo_net = DataParallel(self._get_resnet50()).cuda()
            sketch_net = DataParallel(self._get_resnet50()).cuda()

        if self.fine_tune:
            photo_net_root = self.model_root
            sketch_net_root = self.model_root.replace('photo', 'sketch')

            photo_net.load_state_dict(t.load(photo_net_root, map_location=t.device('cpu')))
            sketch_net.load_state_dict(t.load(sketch_net_root, map_location=t.device('cpu')))

        print('net')
        print(photo_net)

        # triplet_loss = nn.TripletMarginLoss(margin=self.margin, p=self.p).cuda()
        photo_cat_loss = nn.CrossEntropyLoss().cuda()
        sketch_cat_loss = nn.CrossEntropyLoss().cuda()

        my_triplet_loss = TripletLoss().cuda()

        # optimizer
        photo_optimizer = t.optim.Adam(photo_net.parameters(), lr=self.lr)
        sketch_optimizer = t.optim.Adam(sketch_net.parameters(), lr=self.lr)

        if self.vis:
            vis = Visualizer(self.env)

        triplet_loss_meter = AverageValueMeter()
        sketch_cat_loss_meter = AverageValueMeter()
        photo_cat_loss_meter = AverageValueMeter()

        data_loader = TripleDataLoader(self.dataloader_opt)
        dataset = data_loader.load_data()

        for epoch in range(self.epochs):

            print('---------------{0}---------------'.format(epoch))

            if self.test and epoch % self.test_f == 0:

                tester_config = Config()
                tester_config.test_bs = 128
                tester_config.photo_net = photo_net
                tester_config.sketch_net = sketch_net

                tester_config.photo_test = self.photo_test
                tester_config.sketch_test = self.sketch_test

                tester = Tester(tester_config)
                test_result = tester.test_instance_recall()

                result_key = list(test_result.keys())
                vis.plot('recall', np.array([test_result[result_key[0]], test_result[result_key[1]]]),
                              legend=[result_key[0], result_key[1]])
                if self.save_model:
                    t.save(photo_net.state_dict(), self.save_dir + '/photo' + '/photo_' + self.net + '_%s.pth' % epoch)
                    t.save(sketch_net.state_dict(), self.save_dir + '/sketch' + '/sketch_' + self.net + '_%s.pth' % epoch)

            photo_net.train()
            sketch_net.train()

            for ii, data in enumerate(dataset):

                photo_optimizer.zero_grad()
                sketch_optimizer.zero_grad()

                photo = data['P'].cuda()
                sketch = data['S'].cuda()
                label = data['L'].cuda()

                p_cat, p_feature = photo_net(photo)
                s_cat, s_feature = sketch_net(sketch)

                # category loss
                p_cat_loss = photo_cat_loss(p_cat, label)
                s_cat_loss = sketch_cat_loss(s_cat, label)

                photo_cat_loss_meter.add(p_cat_loss.item())
                sketch_cat_loss_meter.add(s_cat_loss.item())

                # triplet loss
                loss = p_cat_loss + s_cat_loss

                # tri_record = 0.
                '''
                for i in range(self.batch_size):
                    # negative
                    negative_feature = t.cat([p_feature[0:i, :], p_feature[i + 1:, :]], dim=0)
                    # print('negative_feature.size :', negative_feature.size())
                    # photo_feature
                    anchor_feature = s_feature[i, :]
                    anchor_feature = anchor_feature.expand_as(negative_feature)
                    # print('anchor_feature.size :', anchor_feature.size())

                    # positive
                    positive_feature = p_feature[i, :]
                    positive_feature = positive_feature.expand_as(negative_feature)
                    # print('positive_feature.size :', positive_feature.size())

                    tri_loss = triplet_loss(anchor_feature, positive_feature, negative_feature)

                    tri_record = tri_record + tri_loss

                    # print('tri_loss :', tri_loss)
                    loss = loss + tri_loss
                '''
                # print('tri_record : ', tri_record)

                my_tri_loss = my_triplet_loss(s_feature, p_feature) / (self.batch_size - 1)
                triplet_loss_meter.add(my_tri_loss.item())
                # print('my_tri_loss : ', my_tri_loss)

                # print(tri_record - my_tri_loss)
                loss = loss + my_tri_loss
                # print('loss :', loss)
                # loss = loss / opt.batch_size

                loss.backward()

                photo_optimizer.step()
                sketch_optimizer.step()

                if self.vis:
                    vis.plot('triplet_loss', np.array([triplet_loss_meter.value()[0], photo_cat_loss_meter.value()[0],
                                                       sketch_cat_loss_meter.value()[0]]),
                             legend=['triplet_loss', 'photo_cat_loss', 'sketch_cat_loss'])

                triplet_loss_meter.reset()
                photo_cat_loss_meter.reset()
                sketch_cat_loss_meter.reset()


from django.db import models
import numpy as np
from django.conf import settings
from keras.preprocessing import image
from tensorflow.keras.models import load_model
import os
from PIL import Image as Img
import tensorflow.compat.v1 as tfv1

# データベーススキーマーImageテーブルのスキーマ
class Image(models.Model):
    # 画像コラム
    picture = models.ImageField()
    # 判別結果コラム
    classified = models.CharField(max_length=200, blank=True)
    # 判定日時コラム
    uploaded = models.DateTimeField(auto_now_add=True)

    # adminページで判定画像DBにアクセスする時の表示フォーマット
    def __str__(self):
        return "Image classified at {}".format(self.uploaded.strftime('%Y-%m-%d %H:%M'))

    def save(self, *args, **kwargs):

        # フルバージョン　２２種類の食材
        # LABELS = ['avogado','apple','banana','bean_sprouts','cabbage','carrot',
        # 'chicken','eggs','grape','green_onions','milk','miso','onion','orange',
        # 'pineapple','peach','potatoes','salt','shrimp','strawberry','tofu','yogurt']

        # ミニバージョン　５種類の食材
        LABELS = ['cabbage', 'carrot', 'milk', 'onion', 'tofu']

        # フロントから送られてきた画像をImgオブジェクトとして保存
        img_pil = Img.open(self.picture)

        # ベースディレクトリ api + media_root + 判定用画像名
        img_path = os.path.join(settings.BASE_DIR, 'media_root/' + str(self.picture))

        # 上のパス上に画像を保存
        img_pil.save(img_path, "JPEG")

        # 保存後に画像を学習時のサイズで読み込み、imageオブジェクトとして保存
        img = image.load_img(img_path, target_size=(224,224))

        # 上のオブジェクトをnumpy配列に変換
        img_array = image.img_to_array(img)

        # モデルの入力フォーマットに合わせ、axis=0の次元を追加（１はデータ数が一個であることを表す）
        to_pred = np.expand_dims(img_array, axis=0) #(1, 225, 225, 3)

        try:
            # モデルの保存パスを指定
            file_model = os.path.join(settings.BASE_DIR, 'model/fridgeCF2_20210808_172513.h5')

            # デフォルトのグラフにアクセス
            graph = tfv1.get_default_graph()

            # グラフ上での演算を指定
            with graph.as_default():
                # VGG16モデルをロードし、保存
                model = load_model(file_model)

                # 各食材の判定確率をコンソール上で確認(デバッグ時確認用)
                # print(dict(zip(LABELS, np.squeeze(model.predict(to_pred)).tolist())))

                # モデルを用いて各食材の判定確率が保存されたnumpy配列をリターン
                pred_prob = model.predict(to_pred)

                # 判定確率が0.8以上の食材があれば、
                if (pred_prob >= 0.8).any():
                    # 一番判定確率が高い食材のインデックスをラベルが保存されているリストと照らし合わせ、食材名を保存
                    pred_ingred = LABELS[np.argmax(model.predict(to_pred))]

                # 判定確率が0.8以上の食材がない場合、
                else:
                    raise Exception

                # classifiedコラムのレコードを判定食材名で保存
                self.classified = str(pred_ingred)
                print(f'classified as {pred_ingred}')

        except Exception as e:
            # デバッグ用にエラーの詳細を出力
            template = "An exception of type {0} occurred. Arguments:\n{1!r}"
            message = template.format(type(e).__name__, e.args)
            print(message)
            print('failed to classify')

            # classifiedコラムのレコードを誤判定として保存
            self.classified = 'failed to classify'

        # 判定終了したイメージファイルをローカルパスから削除（以後DB上で保存管理）
        if os.path.exists(img_path):
            os.remove(img_path)
        else:
            print("The file does not exist")

        # DBのレコードとして挿入し、コミット
        super().save(*args, **kwargs)


import numpy as np
from keras.preprocessing.sequence import pad_sequences
from keras.models import model_from_json

class W2VV_pred(object):
    def __init__(self, model_path, weight_path, text2vec, sent_maxlen, embed_size, bow2vec, w2v2vec):

        self.w2vv_model = model_from_json(open(model_path).read())
        self.w2vv_model.load_weights(weight_path)
        self.w2vv_model.compile(loss='mse', optimizer='rmsprop')
        self.text2vec = text2vec
        self.bow2vec = bow2vec
        self.w2v2vec = w2v2vec
        self.sent_maxlen = sent_maxlen

    # predict a visual feature vector for a given text input
    def predict_one(self, text_input):
        text_embedding = self.text2vec.embedding(text_input)

        bow_embedding = self.bow2vec.embedding(text_input)
        w2v_embedding = self.w2v2vec.embedding(text_input)
        # fasttext_embedding = self.fasttext2vec.embedding(text_input)
        # text string not in the word2vec vocabulary
        if text_embedding is None:
            text_embedding = np.zeros(self.text2vec.ndims)
        if bow_embedding is None:
            bow_embedding = np.zeros(self.bow2vec.ndims)
        if w2v_embedding is None:
            w2v_embedding = np.zeros(self.w2v2vec.ndims)
        #if text_embedding is None or bow_embedding is None or w2v_embedding is None:
        #    return None
        text_embedding = pad_sequences([list(text_embedding)], maxlen=self.sent_maxlen)[0]
        pred_visual_feat_vec = self.w2vv_model.predict([np.array([text_embedding]), np.array([list(bow_embedding) + list(w2v_embedding)])])[0]
        return pred_visual_feat_vec.tolist()

    # predict visual feature vectors for a given list of text input
    # return:
    # success_text_input_index: the index of sucess input text
    # pred_visual_feat_vecs: the predicted visual feature vectors
    def predict_batch(self, text_input_batch):
        success_text_input_index = []
        text_embedding_batch = []
        bow_embedding_batch = []
        for index, text_input in enumerate(text_input_batch):
            text_embedding = self.text2vec.embedding(text_input)
            bow_embedding = self.bow2vec.embedding(text_input)
            w2v_embedding = self.w2v2vec.embedding(text_input)
            if text_embedding is not None and bow_embedding is not None and w2v_embedding is not None:
                success_text_input_index.append(index)
                text_embedding_batch.append(text_embedding)
                bow_embedding_batch.append(list(bow_embedding) + list(w2v_embedding))

        pred_visual_feat_vecs = self.w2vv_model.predict([np.array(text_embedding_batch), np.array(bow_embedding_batch)])

        return success_text_input_index, pred_visual_feat_vecs.tolist()


'''
The model provided in the repo was trained on a combination of FER+ and AffectNet datasets with 75% accuracy on the test set.

To Train:
1. Download FER+ dataset through GitHub.
2. Extract the images of the original FER2013 dataset.
3. Place all Train, Test and Validation images into the single folder in "current_directory/dataset/images"
4. Run this model.py file.

FER+ has less misclassification error than FER2013 as there is no incorrect labelling.
The state-of-the-art techniques used in training FER2013 can be implemented here to increase accuracy
of model. Transfer Learning with other dataset-trained models and Fine-tuning those will lead to higher accuracy.

'''

import pandas as pd

#data
data = pd.read_csv("./dataset/fer2013new.csv")
data_prep = data.drop(['fear','contempt','unknown','NF','Usage','disgust'], axis = 1)
data_prep['Emotion'] = data_prep.drop(['Image name'], axis = 1).idxmax(axis=1, skipna=True) #empty row is deleted
data_prep.rename(columns = {'Image name':'Image_name'}, inplace = True)
data_prep['Emotion'] = data_prep['Emotion'].replace({'neutral':'0','happiness':'1','sadness':'2','surprise':'3','anger':'4'})
file_emotion = pd.Series(data_prep.Emotion.values,index=data_prep.Image_name).to_dict()

import tensorflow as tf
from tensorflow.keras.models import Sequential
from tensorflow.keras.layers import Dense, Dropout, Activation, Flatten
from tensorflow.keras.layers import Conv2D, MaxPooling2D,BatchNormalization
import numpy as np
import os
from keras.preprocessing import image
from keras.utils import to_categorical
from sklearn.model_selection import train_test_split

x = []
y = []

### loading data
labels = {"Neutral":0,"Happy":1,"Sad":2,"Surprise":3,"Angry":4}

for key, value in file_emotion.items():
	if key in os.listdir("./dataset/images"):
		img = image.load_img("./dataset/images/"+key, target_size=(48,48,1), color_mode = "grayscale")
		img = image.img_to_array(img)
		img = img/255.0
		x.append(img)
		y.append(value)
x = np.array(x)

# # Split the data
x_train, x_test, y_train, y_test = train_test_split(x, y, test_size=0.1, shuffle= True)

y_train = to_categorical(y_train)
y_test = to_categorical(y_test)

# CNN model:

model = Sequential()
model.add(Conv2D(64, (3, 3), input_shape=(48,48,1)))
model.add(Activation('relu'))
model.add(BatchNormalization())
model.add(MaxPooling2D(pool_size=(2, 2)))

model.add(Conv2D(128, (3, 3)))
model.add(Dropout(0.3))
model.add(Activation('relu'))
model.add(BatchNormalization())
model.add(MaxPooling2D(pool_size=(2, 2)))

model.add(Conv2D(256, (3, 3)))
model.add(Activation('relu'))
model.add(BatchNormalization())
model.add(MaxPooling2D(pool_size=(2, 2)))

model.add(Conv2D(256, (3, 3)))
model.add(Activation('relu'))
model.add(BatchNormalization())
model.add(MaxPooling2D(pool_size=(2, 2)))

model.add(Flatten())  # this converts our 3D feature maps to 1D feature vectors

model.add(Dense(128))
model.add(Activation('relu'))
model.add(BatchNormalization())
model.add(Dropout(0.25))

model.add(Dense(64))
model.add(Activation('relu'))
model.add(BatchNormalization())

model.add(Dense(5)) #num_classes = 5
model.add(Activation('softmax'))

print(model.summary())

model.compile(loss='categorical_crossentropy',
            optimizer='adam',
            metrics=['accuracy'])

model.fit(x_train, y_train, epochs=15, batch_size=64)
scores = model.evaluate(x_test, y_test)
print("%s: %.2f%%" % (model.metrics_names[1], scores[1]*100))
model.save('./expression.model')


import sys
import numpy as np
from time import clock
from masterboard_utils import CalibrationController, mbs, GotoController

class RobotHAL():
    ''' This class provide a robot specific access to the solo generic hardware'''

    def __init__(self, interfaceName="eth0", dt=0.001):
        self.isInitialized = False
        self.InitRobotSpecificParameters()
        assert len(self.motorToUrdf) == self.nb_motors
        assert len(self.motorSign) == self.nb_motors
        assert self.nb_motors % 2 == 0
        # TODO assert mapping..
        assert self.maximumCurrent >= 0

        # decide how to search for the index, given that we are close to the real zero
        searchStrategy = self.nb_motors * [CalibrationController.ALTERNATIVE]

        # to be in [-2pi;+2pi]
        self.encoderOffsets = np.fmod(self.encoderOffsets, 2*np.pi)
        # to be in [-pi;+pi]
        self.encoderOffsets[self.encoderOffsets > +np.pi] -= 2*np.pi
        self.encoderOffsets[self.encoderOffsets < -np.pi] += 2*np.pi

        for i in range(self.nb_motors):
            if (self.encoderOffsets[i] > (np.pi/2.0)):
                searchStrategy[i] = CalibrationController.POSITIVE
            elif (self.encoderOffsets[i] < - (np.pi/2.0)):
                searchStrategy[i] = CalibrationController.NEGATIVE
        print(searchStrategy)
        print(self.encoderOffsets)
        self.t = 0
        self.cpt = 0
        self.last = 0
        self.dt = dt
        self.nb_motorDrivers = int(self.nb_motors/2)
        self.gearRatioSigned = np.zeros(8)
        self.gearRatioSigned = self.motorSign * self.gearRatio
        self.jointKtSigned = self.motorKt * self.gearRatioSigned  # Nm(joint)/A(motor)

        self.q_mes = np.zeros(self.nb_motors)
        self.v_mes = np.zeros(self.nb_motors)
        self.torquesFromCurrentMeasurment = np.zeros(self.nb_motors)
        self.baseAngularVelocity = np.zeros(3)
        self.baseOrientation = np.array([0., 0., 0., 1.])
        self.baseLinearAcceleration = np.zeros(3)
        self.hardware = mbs.MasterBoardInterface(interfaceName)
        self.calibCtrl = CalibrationController(self.hardware, self.nb_motors, self.dt, Kd=0.01, Kp=3.0 ,searchStrategy=searchStrategy)
        self.gotoCtrl = GotoController(self.hardware, self.nb_motors, self.dt, Kd=0.01, Kp=3.0)

    def InitRobotSpecificParameters(self):
        '''
        This function initialises all robot specific parameters
        This function **must** be overloaded in a child class for different robots
        '''
        raise RuntimeError("This class is an abstract class. Please overload this method.")

    def Init(self,calibrateEncoders=False, q_init=None):
        # Initialization of the interface between the computer and the master board and the master board itself
        self.hardware.Init()
        self.EnableAllMotors()
        self.isInitialized = True

        self.InitMasterBoard() # Initialization of the master board

        if not self.AreAllDriversConnected():
            self.hardware.Stop()
            raise RuntimeError("Not all declared motor drivers are connected.") # TODO replace RuntimeError by custom exception

        if q_init is not None:
            # Define initial configuration after calibration
            assert (max(abs(q_init))<2*np.pi)
            # Convert to motor angle:
            motor_angle_init = np.zeros(self.nb_motors)
            for i in range(self.nb_motors):
                motor_angle_init[i] = q_init[self.motorToUrdf[i]] * self.gearRatioSigned[i]
            # Change the target of the controller
            self.gotoCtrl.FinalPosition = motor_angle_init
            self.T_move = 3.

        if calibrateEncoders:
            for i in range(self.nb_motors):
                self.hardware.GetMotor(i).SetPositionOffset(self.encoderOffsets[i])
                self.hardware.GetMotor(i).enable_index_offset_compensation = True
            print("Running calibration...")
            self.RunHommingRoutine()
            print("End Of Calibration")

    @staticmethod
    def EulerToQuaternion(roll_pitch_yaw):
        roll, pitch, yaw = roll_pitch_yaw
        sr = np.sin(roll/2.)
        cr = np.cos(roll/2.)
        sp = np.sin(pitch/2.)
        cp = np.cos(pitch/2.)
        sy = np.sin(yaw/2.)
        cy = np.cos(yaw/2.)
        qx = sr * cp * cy - cr * sp * sy
        qy = cr * sp * cy + sr * cp * sy
        qz = cr * cp * sy - sr * sp * cy
        qw = cr * cp * cy + sr * sp * sy
        return [qx, qy, qz, qw]
        # return [-qy, -qx, qz, -qw]  # return with a rotation of 90dege around Z

    def SetDesiredJointTorque(self, tau):
        for i in range(self.nb_motors):
            cur = tau[self.motorToUrdf[i]] / self.jointKtSigned[i] #todo missing gear
            # cur = np.clip(cur,-self.maximumCurrent,self.maximumCurrent) #takes too long !
            if (cur > self.maximumCurrent):
                cur = self.maximumCurrent
            elif (cur < -self.maximumCurrent):
                cur = -self.maximumCurrent
            if self.hardware.GetMotor(i).IsEnabled():
                self.hardware.GetMotor(i).SetCurrentReference(cur)
            else:
                self.hardware.GetMotor(i).SetCurrentReference(0)
        return

    def SetDesiredJointPDgains(self, kp, kd):
        for i in range(self.nb_motors):
            motor_kp = kp[self.motorToUrdf[i]] / (self.motorKt[i]*self.gearRatio[i]*self.gearRatio[i])
            motor_kd = kd[self.motorToUrdf[i]] / (self.motorKt[i]*self.gearRatio[i]*self.gearRatio[i])
            if self.hardware.GetMotor(i).IsEnabled():
                #self.hardware.GetMotor(i).SetKp(motor_kp)
                #self.hardware.GetMotor(i).SetKd(motor_kd)
                self.hardware.GetMotor(i).kp=motor_kp
                self.hardware.GetMotor(i).kd=motor_kd
            else:
                self.hardware.GetMotor(i).SetCurrentReference(0.)
                #self.hardware.GetMotor(i).SetKp(0.)
                #self.hardware.GetMotor(i).SetKd(0.)
                self.hardware.GetMotor(i).kp=0.
                self.hardware.GetMotor(i).kd=0.
        return

    def SetDesiredJointPosition(self,pos):
        for i in range(self.nb_motors):
            motor_pos = pos[self.motorToUrdf[i]] * self.gearRatioSigned[i]
            #self.hardware.GetMotor(i).SetPositionReference(motor_pos)
            self.hardware.GetMotor(i).position_ref = motor_pos
        return

    def SetDesiredJointVelocity(self,vel):
        for i in range(self.nb_motors):
            motor_vel = vel[self.motorToUrdf[i]] * self.gearRatioSigned[i]
            #self.hardware.GetMotor(i).SetVelocityReference(motor_vel)
            self.hardware.GetMotor(i).velocity_ref = motor_vel
        return

    def EnableAllMotors(self):
        for i in range(self.nb_motorDrivers):
            #todo reset local PD+ param?
            self.hardware.GetDriver(i).motor1.SetCurrentReference(0)
            self.hardware.GetDriver(i).motor2.SetCurrentReference(0)
            self.hardware.GetDriver(i).motor1.kp=0.
            self.hardware.GetDriver(i).motor2.kp=0.
            self.hardware.GetDriver(i).motor1.kd=0.
            self.hardware.GetDriver(i).motor2.kd=0.
            self.hardware.GetDriver(i).motor1.Enable()
            self.hardware.GetDriver(i).motor2.Enable()
            self.hardware.GetDriver(i).EnablePositionRolloverError()
            self.hardware.GetDriver(i).SetTimeout(5)
            self.hardware.GetDriver(i).Enable()
        return

    def SendInit(self, WaitEndOfCycle=True):
        '''This (possibly blocking) fuction will send an init packet to the robot'''
        assert self.isInitialized, "The Robot HAL is not initialized. You have to call init() first"
        self.hardware.SendInit()
        if WaitEndOfCycle:
            self.WaitEndOfCycle()

    def InitMasterBoard(self):
        while not self.hardware.IsTimeout() and not self.hardware.IsAckMsgReceived():
            self.SendInit(WaitEndOfCycle=True)

    def UpdateMeasurment(self):
        '''This function will parse the last sensor packet, and convert position and velocity according to the robot actuation parameters'''
        self.hardware.ParseSensorData()
        for i in range(self.nb_motors):
            if i % 2 == 0:
                error_code = self.hardware.GetDriver(i//2).GetErrorCode()
                if  error_code != 0:
                    self.hardware.Stop()
                    raise RuntimeError("Driver {} returned error {:#x}.".format(i//2, error_code)) # TODO replace RuntimeError by custom exception

            if self.hardware.GetMotor(i).IsEnabled():
                self.q_mes[self.motorToUrdf[i]] = self.hardware.GetMotor(i).GetPosition()/self.gearRatioSigned[i]
                self.v_mes[self.motorToUrdf[i]] = self.hardware.GetMotor(i).GetVelocity()/self.gearRatioSigned[i]
                self.torquesFromCurrentMeasurment[self.motorToUrdf[i]] = self.hardware.GetMotor(i).GetCurrent()*self.jointKtSigned[i]

        # Angular velocities of the base from IMU Gyroscope
        self.baseAngularVelocity[:] = self.rotateImuVectors(
            [self.hardware.imu_data_gyroscope(0), self.hardware.imu_data_gyroscope(1), self.hardware.imu_data_gyroscope(2)])

        # Orientation of the base from IMU Estimation Filter
        self.baseOrientation[:] = self.rotateImuOrientation(self.EulerToQuaternion(
            [self.hardware.imu_data_attitude(0), self.hardware.imu_data_attitude(1), self.hardware.imu_data_attitude(2)]))

        # Linear Acceleration of the base from IMU Estimation Filter, note the rotation !
        self.baseLinearAcceleration[:] = self.rotateImuVectors(
            [self.hardware.imu_data_linear_acceleration(0), self.hardware.imu_data_linear_acceleration(1), self.hardware.imu_data_linear_acceleration(2)])

        return

    def SendCommand(self, WaitEndOfCycle=True):
        '''This (possibly blocking) fuction will send the command packet to the robot'''
        assert self.isInitialized, "The Robot HAL is not initialized. You have to call init() first"
        self.hardware.SendCommand()
        if WaitEndOfCycle:
            self.WaitEndOfCycle()

    def AreAllDriversConnected(self):
        '''Test if all motor drivers are connected'''
        for i in range(self.nb_motorDrivers):
            if not (self.hardware.GetDriver(i).IsConnected()):
                return False
        return True

    def AreAllMotorsReady(self):
        '''Test if all motors are enabled and ready'''
        for i in range(self.nb_motors):
            if not (self.hardware.GetMotor(i).IsEnabled() and self.hardware.GetMotor(i).IsReady()):
                return False
        return True

    def RunHommingRoutine(self):
        '''This Blocking function will perform a homing Routine'''
        state = 0
        while (state != 3):
            if (self.hardware.IsTimeout()):
                return
            self.UpdateMeasurment()
            if (state == 0):
                if (self.AreAllMotorsReady()):
                    state = 1
            elif (state == 1):
                if (self.calibCtrl.ManageCalibration()):
                    state = 2
            elif (state == 2):
                if (self.gotoCtrl.ManageControl()):
                    state = 3
            self.SendCommand(WaitEndOfCycle=True)

        return

    def WaitEndOfCycle(self):
        '''This Blocking fuction will wait for the end of timestep cycle (dt).'''
        while(1):
            if((clock() - self.last) >= self.dt):
                self.last = clock()
                self.cpt += 1
                self.t += self.dt
                return

    def Print(self):
        print(chr(27) + "[2J")
        self.hardware.PrintIMU()
        self.hardware.PrintADC()
        self.hardware.PrintMotors()
        self.hardware.PrintMotorDrivers()
        #self.hardware.PrintCmdStats()
        #self.hardware.PrintSensorStats()
        print("q_mes = ", self.q_mes)
        print("v_mes = ", self.v_mes)
        sys.stdout.flush()