from __future__ import annotations

import functools
import inspect
from itertools import groupby
from typing import Any, Callable, Dict, List, Sequence, Tuple, Type, TypeVar, Union

from baize.asgi import FormData
from pydantic import BaseConfig, BaseModel, ValidationError, create_model

from indexpy.exceptions import RequestValidationError
from indexpy.requests import request
from indexpy.utils import safe_issubclass

from .fields import FieldInfo, RequestInfo, Undefined

CallableObject = TypeVar("CallableObject", bound=Callable)

def create_model_config(title: str = None, description: str = None) -> Type[BaseConfig]:
    class ExclusiveModelConfig(BaseConfig):
        schema_extra = {
            k: v
            for k, v in {"title": title, "description": description}.items()
            if v is not None
        }

    return ExclusiveModelConfig

def parse_signature(function: CallableObject) -> CallableObject:
    sig = inspect.signature(function)

    __parameters__: Dict[str, Any] = {
        key: {} for key in ["path", "query", "header", "cookie", "body"]
    }
    __exclusive_models__: Dict[BaseModel, str] = {}

    for name, param in sig.parameters.items():
        if not isinstance(param.default, FieldInfo):
            continue

        if param.POSITIONAL_ONLY:
            raise TypeError(
                f"Parameter {name} cannot be defined as positional only parameters."
            )

        default = param.default
        annotation = param.annotation

        if getattr(default, "exclusive", False):
            if safe_issubclass(annotation, BaseModel):
                model = annotation
            else:
                model = create_model(
                    "temporary_exclusive_model",
                    __config__=create_model_config(default.title, default.description),
                    __root__=(annotation, ...),
                )
            __parameters__[default._in] = model
            __exclusive_models__[model] = name
        else:
            if safe_issubclass(__parameters__[default._in], BaseModel):
                raise RuntimeError(
                    f"{default._in.capitalize()}(exclusive=True) "
                    f"and {default._in.capitalize()} cannot be used at the same time"
                )
            if annotation == param.empty:
                annotation = Any
            __parameters__[default._in][name] = (annotation, default)

    for key, params in filter(
        lambda kv: kv[1],
        ((key, __parameters__.pop(key)) for key in tuple(__parameters__.keys())),
    ):
        if safe_issubclass(params, BaseModel):
            model = params
        else:
            model = create_model("temporary_model", **params)
        __parameters__[key] = model

    if "body" in __parameters__:
        setattr(function, "__request_body__", __parameters__.pop("body"))

    if __parameters__:
        setattr(function, "__parameters__", __parameters__)

    if __exclusive_models__:
        setattr(function, "__exclusive_models__", __exclusive_models__)

    request_attrs = {
        name: param.default
        for name, param in sig.parameters.items()
        if isinstance(param.default, RequestInfo)
    }
    if request_attrs:
        setattr(function, "__request_attrs__", request_attrs)

    __signature__ = inspect.Signature(
        parameters=[
            param
            for param in sig.parameters.values()
            if not isinstance(param.default, (FieldInfo, RequestInfo))
        ],
        return_annotation=sig.return_annotation,
    )
    setattr(function, "__signature__", __signature__)

    return function

def _merge_multi_value(
    items: Sequence[Tuple[str, Any]]
) -> Dict[str, Union[str, List[str]]]:
    """
    If there are values with the same key value, they are merged into a List.
    """
    return {
        k: v_list if len(v_list) > 1 else v_list[0]
        for k, v_list in (
            (k, list(v for _, v in kv_iter))
            for k, kv_iter in (
                lambda iterable, key: groupby(sorted(iterable, key=key), key=key)
            )(items, lambda kv: kv[0])
        )
    }

async def verify_params(handler: CallableObject) -> CallableObject:
    parameters: Dict[str, BaseModel] = getattr(handler, "__parameters__", None)
    request_body: BaseModel = getattr(handler, "__request_body__", None)
    request_attrs: Dict[str, RequestInfo] = getattr(handler, "__request_attrs__", None)
    if not (parameters or request_body or request_attrs):
        return handler

    exclusive_models: Dict[Type[BaseModel], str] = getattr(
        handler, "__exclusive_models__", {}
    )

    data: List[Any] = []
    kwargs: Dict[str, Any] = {}

    try:
        # try to get parameters model and parse
        if parameters:
            if "path" in parameters:
                data.append(parameters["path"].parse_obj(request.path_params))

            if "query" in parameters:
                data.append(
                    parameters["query"].parse_obj(
                        _merge_multi_value(request.query_params.multi_items())
                    )
                )

            if "header" in parameters:
                data.append(parameters["header"].parse_obj(request.headers))

            if "cookie" in parameters:
                data.append(parameters["cookie"].parse_obj(request.cookies))

        # try to get body model and parse
        if request_body:
            _body_data = await request.data()
            if isinstance(_body_data, FormData):
                _body_data = _merge_multi_value(_body_data.multi_items())
            data.append(request_body.parse_obj(_body_data))

        # try to get request instance attributes
        if request_attrs:
            for name, info in request_attrs.items():
                try:
                    value: Any = functools.reduce(
                        lambda attr, name: getattr(attr, name),
                        (info.alias or name).split("."),
                        request,
                    )
                except AttributeError:
                    if info.default is not Undefined:
                        value = info.default
                    elif info.default_factory is not None:
                        value = info.default_factory()
                    else:
                        raise
                kwargs[name] = (await value) if inspect.isawaitable(value) else value

    except ValidationError as e:
        raise RequestValidationError(e)

    for _data in data:
        if _data.__class__.__name__ == "temporary_model":
            kwargs.update(_data.dict())
        elif _data.__class__.__name__ == "temporary_exclusive_model":
            kwargs[exclusive_models[_data.__class__]] = _data.__root__
        else:
            kwargs[exclusive_models[_data.__class__]] = _data
    return functools.partial(handler, **kwargs)  # type: ignore

def create_new_callback(callback: CallableObject) -> CallableObject:
    @functools.wraps(callback)
    async def callback_with_auto_bound_params(*args, **kwargs):
        p = await verify_params(callback)
        return await p(*args, **kwargs)

    return callback_with_auto_bound_params  # type: ignore

has_wrapped_by_auto_params = lambda function: (
    hasattr(function, "__parameters__")
    or hasattr(function, "__request_body__")
    or hasattr(function, "__request_attrs__")
)

def auto_params(handler: CallableObject) -> CallableObject:
    if inspect.isclass(handler) and hasattr(handler, "__methods__"):
        for method in map(lambda x: x.lower(), handler.__methods__):  # type: ignore
            function = getattr(handler, method)
            if has_wrapped_by_auto_params(function):
                continue
            callback = parse_signature(function)
            setattr(handler, method, create_new_callback(callback))
        return handler
    elif inspect.iscoroutinefunction(handler):
        callback = parse_signature(handler)
        return create_new_callback(callback)
    else:
        return handler


# -*- coding: utf-8 -*-
"""
Created on Sun Dec 10 00:25:29 2017

@author: James Jiang
"""

elements = list(range(256))
lengths_not_ascii = [130, ',', 126, ',', 1, ',', 11, ',', 140, ',', 2, ',', 255, ',', 207, ',', 18, ',', 254, ',', 246, ',', 164, ',', 29, ',', 104, ',', 0, ',', 224]
lengths = []

for i in range(len(lengths_not_ascii)):
    digits = [j for j in str(lengths_not_ascii[i])]
    for k in range(len(digits)):
        lengths.append(ord(digits[k]))

lengths.extend([17, 31, 73, 47, 23])

skip = 0
elements_pointer = 0

for count in range(64):
    for i in range(len(lengths)):
        current_length = lengths[i]
        if elements_pointer + current_length >= len(elements):
            sublist1 = elements[elements_pointer:]
            sublist2 = elements[0:current_length - len(sublist1)]
            sublist = sublist1 + sublist2
        else:
            sublist = elements[elements_pointer:elements_pointer + current_length]
        sublist.reverse()
        if elements_pointer + current_length >= len(elements):
            elements[elements_pointer:] = sublist[0:len(sublist1)]
            elements[0:current_length - len(sublist1)] = sublist[len(sublist1):]
        else:
            elements[elements_pointer:elements_pointer + current_length] = sublist
        if elements_pointer + (current_length + skip) >= len(elements):
            elements_pointer = elements_pointer + current_length + skip - len(elements)
            while True:
                if elements_pointer in range(len(elements)):
                    break
                else:
                    elements_pointer -= len(elements)
        else:
            elements_pointer += (current_length + skip)
        skip += 1

dense_hash_list = []

for i in range(0, len(elements), 16):
    hash = elements[i] ^ elements[i + 1] ^ elements[i + 2] ^ elements[i + 3] ^ elements[i + 4] ^ elements[i + 5] ^ elements[i + 6] ^ elements[i + 7] ^ elements[i + 8] ^ elements[i + 9] ^ elements[i + 10] ^ elements[i + 11] ^ elements[i + 12] ^ elements[i + 13] ^ elements[i + 14] ^ elements[i + 15]
    dense_hash_list.append(hash)

knot_hash = ''

for i in range(0, len(dense_hash_list)):
    string = hex(dense_hash_list[i])[2:]
    if len(string) == 1:
        string = '0' + string
    knot_hash += string

print(knot_hash)


#!/usr/bin/env python
# -*- coding:utf-8 -*-

import os
dstPath = 'E:\\CodeProject'
txt = 'getString'
fileExt = '.h'
def SearchText(dir, txt, fileExt):
    list = os.listdir(dir)
    for line in list:
        filepath = os.path.join(dir, line)
        if os.path.isdir(filepath):
            SearchText(filepath, txt, fileExt)

        if filepath.endswith(fileExt):
            try:
                with open(filepath, 'r', encoding='gbk') as f:
                    lines = f.readlines()
                    #print(lines)
                    if str(lines).find(txt) != -1:
                        print(filepath)
                        print('------')
            except:
                print("xxxxxxxxxxxxxxxxxxxxx:    %s" % filepath)

SearchText(dstPath, txt, fileExt)


import asyncio
import io
import logging
from pathlib import Path

import aiohttp
from aiogram import Bot, Dispatcher, types

from .config import settings

logging.basicConfig(level=logging.INFO)

logger = logging.getLogger(__name__)

with Path(__file__).parent.joinpath("templates/message.html").open() as f:
    email_template = f.read()

async def send_email(message: types.Message):
    if settings.TG_HASTAG_ALL in message.html_text:
        email_to = settings.EMAIL_TO_ALL
    else:
        email_to = settings.EMAIL_TO

    # удаляем хештег и переносы строк в конце сообщения
    email_text = (
        message.html_text.replace(f"#{settings.TG_HASTAG_ALL}", "")
        .replace(f"#{settings.TG_HASTAG}", "")
        .rstrip("\n")
    )

    async with aiohttp.ClientSession() as session:
        data = aiohttp.FormData()
        data.add_field("from", settings.EMAIL_FROM)
        data.add_field("to", email_to)
        data.add_field("subject", settings.EMAIL_SUBJECT)
        data.add_field(
            "html",
            email_template.replace("%message%", email_text).replace("\n", "<br>"),
        )
        if message.document is not None:
            data.add_field(
                "attachment",
                await message.document.download(destination_file=io.BytesIO()),
                filename=message.document.file_name,
            )
        params = {
            "auth": aiohttp.BasicAuth("api", settings.MAILGUN_API_KEY),
            "data": data,
        }
        for _ in range(5):
            resp = await session.post(
                f"https://api.mailgun.net/v3/{settings.EMAIL_DOMAIN}/messages", **params
            )
            if resp.status == 200:
                break
            logger.warning("Mailgun response status: %s", resp.status)
            await asyncio.sleep(3)

def create_bot() -> Dispatcher:
    bot = Bot(token=settings.TG_BOT_TOKEN, parse_mode=types.ParseMode.HTML)
    dp = Dispatcher(bot)
    dp.register_channel_post_handler(
        send_email,
        hashtags=[settings.TG_HASTAG, settings.TG_HASTAG_ALL],
        chat_id=settings.TG_CHAT_ID,
        content_types=[types.ContentType.TEXT, types.ContentType.DOCUMENT],
    )
    return dp


import os
import shutil
import zipfile
from os.path import join, getsize

def zip_file(src_dir):
    # 压缩指定文件夹
    zip_name = src_dir + '.zip'
    # src_dir: 压缩的文件夹路径；zip_name: 压缩后zip文件的路径及名称
    z = zipfile.ZipFile(zip_name, 'w', zipfile.ZIP_DEFLATED)
    for dirpath, dirnames, filenames in os.walk(src_dir):
        fpath = dirpath.replace(src_dir,'')
        fpath = fpath and fpath + os.sep or ''
        for filename in filenames:
            z.write(os.path.join(dirpath, filename), fpath + filename)
            print ('==压缩成功==')
    z.close()

def unzip_file(zip_src, dst_dir):
    #zip_src: zip文件的全路径，dst_dir: 要解压到的目的文件夹
    r = zipfile.is_zipfile(zip_src)
    if r:
        fz = zipfile.ZipFile(zip_src, 'r')
        for file in fz.namelist():
            fz.extract(file,dst_dir)
    else:
        print("This is not zip")

"""
  python其他操作文件方法：
    1.剪切（移动）文件到指定目录
      shutil.move(filename, dst_dir)
    2.删除文件夹
      shutil.rmtree(src_dir)
    3.删除指定文件
      os.remove(file_src)
    4.新建文件夹
      os.mkdir(dst_dir)
    5.遍历文件夹
      for filename in os.listdir(src_dir)
    6.复制文件
      shutil.copyfile(src_file,dst_file)
    7.获取文件夹大小
      def get_dir_size(dir_path):
        size = 0L
        for root, dirs, files in os.walk(dir_path):
          size += sum([getsize(join(root, name)) for name in files])
        return size
      可以根据文件大小做出不同的判断：
        file_size = get_dir_size(DATA_PATH)
        max_size = file_size /1024/ 1024 #获取以MB为单位的值
        if max_size < 100:
          pass

"""


""" Order factory to choose order status according to
the service provider
"""
import logging

from django.conf import settings

from oms.custom_requests import GoogleDistanceApiRequest
from order import models as order_model
from service_provider.opinio.functions import OrderStatus

logger = logging.getLogger('common')
logger = logging.LoggerAdapter(logger, settings.LOGGING_EXTRA)

def order_status(workflow):
    """ get the order status on the basis of
    service provider
    Args:
        workflow : workflow instance
    return:
        response_content : parsed response data

    """
    if workflow.state in settings.SYSTEM_STATUS: return {}
    order_event = order_model.get_order_event(workflow.order, workflow.state)
    if order_event:
        additional_data = order_event[0].additional_data
        if 'order_code' or 'state' in additional_data:
            additional_data.pop('order_code', None), additional_data.pop('state', None)
        return additional_data
    return {}

def order_serviceability(order_payload):
    """ get the order status on the basis of
    service provider
    Args:
        merchant_id : merchant id,
        client_id : client id
    return:
        response_content : parsed response data

    """
    try:
        from oms.custom_requests import MerchantRequest
        client_id = order_payload['client_id']
        merchant_id = order_payload['order_details']['pickup']['merchant_id']
        r = MerchantRequest(merchant_id, client_id)
        response = r.send()
        try:
            merchant_id = response.json()[0]['merchant_id']
            if merchant_id:
                from service_provider.opinio.functions import OrderServiceabilityCheck
                order_serviceability = OrderServiceabilityCheck(merchant_id=merchant_id)
                response = order_serviceability.send()
                import ast
                response_content = ast.literal_eval(response.content)
                if response_content['GP_availability'] == 1 or response_content['GP_availability'] == 0:
                    return 1
                else:
                    return 0
            else:
                return
        except:
            return
    except:
        return

def get_order_delivery_distance_time(origin_lat, origin_long, des_lat, des_long,departure_time=None):
    """ get order delivery time accoring to their lat long
    Args:
        origin_lat : origin lattitude
        origin_long : origin longitude
        des_lat : destination lattitude
        des_long : destination longitude
        departure_time : time when fleet departs
    """
    req = GoogleDistanceApiRequest(origin_lat, origin_long, des_lat, des_long, departure_time)
    response = req.send()
    logger.info("google api response status  is %s", response)
    import ast
    response_content = ast.literal_eval(response.content)
    logger.info("google api response is %s", response_content)
    distance = response_content['rows'][0].get('elements')[0].get('distance').get('value')
    if departure_time:
        duration = response_content['rows'][0].get('elements')[0].get('duration_in_traffic').get('value')
    else:
        duration = response_content['rows'][0].get('elements')[0].get('duration').get('value')
    return distance, duration


# William La Cava
# mock experiment for comparing recommenders.
import pdb
import os
import pandas as pd
import numpy as np
import argparse
from ai.recommender.average_recommender import AverageRecommender
from ai.recommender.random_recommender import RandomRecommender
# from ai.recommender.meta_recommender import MetaRecommender
from ai.recommender.knn_meta_recommender import KNNMetaRecommender
# from ai.recommender.mlp_meta_recommender import MLPMetaRecommender
from ai.recommender.svd_recommender import SVDRecommender
from ai.recommender.surprise_recommenders import *

from joblib import Parallel, delayed
from collections import OrderedDict
from mock_experiment.mf_utils import local_get_metafeatures, update_dataset_mf
import warnings
warnings.simplefilter("ignore")
# define a comparison function that tests a recommender on the datasets,
#using an intial knowledge base.
def run_experiment(rec,data_idx,n_recs,trial,knowledge_base,ml_p,n_init, iters):
    """generates recommendations for datasets, using the first n_init as knowledge base."""
    # set seed
    np.random.seed(trial)
    results = []
    kwargs = {'metric':'bal_accuracy','ml_p':ml_p}
    # if rec == 'svd':
    #     kwargs.update({'datasets':knowledge_base.dataset.unique()})
    rec_choice = {'random': RandomRecommender,
            'average': AverageRecommender,
            'knnmeta': KNNMetaRecommender,
            'svd': SVDRecommender,
            'cocluster': CoClusteringRecommender,
            'knnmeans': KNNWithMeansRecommender,
            'knnml': KNNMLRecommender,
            'knndataset': KNNDatasetRecommender,
            'slopeone': SlopeOneRecommender
            }
    # rec_choice = {'random': RandomRecommender,
    #         'average': AverageRecommender,
    #         # 'meta': MetaRecommender,
    #         # 'mlp': MLPMetaRecommender,
    #         'knn': KNNMetaRecommender,
    #         'svd': SVDRecommender
    #         }

    recommender = rec_choice[rec](**kwargs)
    #pdb.set_trace()
    #################################################### load first n_init results into recommender
    subset_ml = np.random.choice(ml_p['algorithm'].unique(), size= n_init)
    train_subset = []
    for s in subset_ml:
        train_subset.append(np.random.choice(
                knowledge_base.loc[knowledge_base.algorithm==s].index))
    print('setting training data for recommender:')
    tmp = knowledge_base.iloc[train_subset]
    for _,row in tmp.iterrows():
        print(row.algorithm,':',row.dataset,':',row.bal_accuracy)

    init_data = []
    init_data_mf = []
    # for i in train_subset:
        # init_data.append(knowledge_base.loc[knowledge_base['dataset']==i])
        # init_df = pd.concat(init_data)
    init_df = knowledge_base.iloc[train_subset]
    # get the metafeatures for the initial datasets
    for i,_ in init_df.groupby('dataset'):
        init_data_mf.append(local_get_metafeatures(i))

    dataset_mf = pd.concat(init_data_mf).set_index('dataset')
    print('initial training on',len(init_df),'results')
    recommender.update(init_df, dataset_mf)
    ####################################################

    ########################################################################## main experiment loop
    datasets = data_idx
    # loop thru rest of datasets
    # for it,dataset in enumerate(rec_subset):
    for it, dataset in data_idx:
        holdout_accuracy_lookup = knowledge_base.loc[
                knowledge_base['dataset'] == dataset].set_index(
            ['algorithm', 'parameter_hash']).loc[:, 'bal_accuracy'].to_dict()
        holdout_rank_lookup = knowledge_base.loc[
                knowledge_base['dataset'] == dataset].set_index(
            ['algorithm', 'parameter_hash']).loc[:, 'ranking'].to_dict()

        print('generating ',n_recs,'recommendations for',dataset)

        # for each dataset, generate a recommendation
        mls, ps, scores = recommender.recommend(dataset_id=dataset,
                                        n_recs=n_recs,
                                        dataset_mf=local_get_metafeatures(dataset)
                                            )
        print('got',len(mls),'recs')
        updates = []
        for i in np.arange(len(mls)):
            ml = mls[i]
            p = ps[i]
            phash = hash(frozenset(p.items()))

            print('recommending',ml,'with',p,'for',dataset)
            if (ml,phash) not in holdout_accuracy_lookup:
                raise ValueError((ml,phash),'not found')

            # n = n+1
            # retreive the performance of the recommended learner
            actual_score = holdout_accuracy_lookup[(ml, phash)]
            actual_ranking = holdout_rank_lookup[(ml,phash)]
            # find all top ranking algorithms
            dataset_results = knowledge_base.loc[knowledge_base['dataset'] == dataset]
            best_score = dataset_results['bal_accuracy'].max()
            best_algs = dataset_results.loc[dataset_results['ranking']==1]['algorithm'].unique()
            best_algorithm = '|'.join(list(best_algs))
            # Update the recommender with the score from its latest guess
            updates.append(pd.DataFrame(data={'dataset': [dataset],
                                               'algorithm': [ml],
                                               'parameters': [p],
                                               'bal_accuracy': [actual_score]})
                          )

            # store the trial, iteration, dataset, recommender, ml rec, param rec,bal_accuracy
            results.append({'trial':trial,
                            'iteration':it,
                            'n_recs':n_recs,
                            'n_init':n_init,
                            'iters':iters,
                            'recommender':rec,
                            'dataset':dataset,
                            'ml-rec':ml,
                            'p-rec':p,
                            'score-rec':scores[0],
                            'bal_accuracy':actual_score,
                            'max_bal_accuracy':best_score,
                            'best_algorithm':best_algorithm,
                            'ranking':actual_ranking,
                            'delta_bal_accuracy':(best_score-actual_score)/best_score})

        print('updating recommender...')
        if len(updates)>0:
            update_record = pd.concat(updates)
            dataset_mf = update_dataset_mf(dataset_mf, update_record)
            recommender.update(update_record,dataset_mf)
        else:
            print('WARNINING: got no updates for',dataset,'. There are ',
                  len([r for r in results if r['dataset'] == dataset]),
                  'results for this dataset already and',len(ml_p),
                  'unique ml+p combos')

    ########################################################################## main experiment loop
    return results

if __name__ == '__main__':
    """run experiment"""

    parser = argparse.ArgumentParser(description='Run a PennAI a recommender experiment.',
                                     add_help=False)
    parser.add_argument('-h','--help',action='help',
                        help="Show this help message and exit.")
    # parser.add_argument('-rec',action='store',dest='rec',default='random',
    #                     help='Recommender to run.')
    parser.add_argument('-rec',action='store',dest='rec',default='random',
            choices = ['random','average','knnmeta','svd','cocluster','knnmeans',
                       'knnml','knndataset','slopeone'],
            help='Recommender algorithm options.')
    parser.add_argument('-n_recs',action='store',dest='n_recs',type=int,default=1,help='Number of '
                        ' recommendations to make at a time. If zero, will send continous '
                        'recommendations until AI is turned off.')
    parser.add_argument('-v','-verbose',action='store_true',dest='verbose',default=False,
                        help='Print out more messages.')
    parser.add_argument('-n_init',action='store',dest='n_init',type=int,default=10,
                        help='Number of initial datasets to seed knowledge database')
    parser.add_argument('-iters',action='store',dest='iters',type=int,default=100,
                        help='Number of initial datasets to seed knowledge database')
    parser.add_argument('-t',action='store',dest='trial',type=int,default=0,
                        help='Trial number')
    parser.add_argument('-data',action='store',dest='KNOWL',type=str,
                        default='mock_experiment/simulation_experiment.tsv',
                        help='knowledge base')
    parser.add_argument('-resdir',action='store',dest='RESDIR',type=str,
                        default='results',
                        help='results directory')

    args = parser.parse_args()

    # load knowledge base(s)
    if ',' in args.KNOWL:
        data_files = args.KNOWL.split(',')
    else:
        data_files = [args.KNOWL]

    for data_file in data_files:
        print(70*'=','\n','loading',data_file,'\n'+70*'=','\n')
        knowledge_base = pd.read_csv(data_file,
                                compression='gzip', sep='\t').fillna('')#,
        ml_p = knowledge_base.loc[:,['algorithm','parameters']].drop_duplicates()
        ml_p['parameters'] = ml_p['parameters'].apply(
                lambda x: eval(x))
        knowledge_base['parameters'] = knowledge_base['parameters'].apply(
                lambda x: eval(x))
        knowledge_base['parameter_hash'] = knowledge_base['parameters'].apply(
                lambda x: hash(frozenset(x.items())))

        print('len ml_p:',len(ml_p))
        data_idx = np.unique(knowledge_base['dataset'])  # datasets

        # output file
        out_file = ('mock_experiment/' + args.RESDIR + '/experiment_'
                    + data_file.split('/')[-1].split('.')[0]
                    + '_rec-{}'.format(args.rec)
                    + '_ninit-{}'.format(args.n_init)
                    + '_nrecs-{}'.format(args.n_recs)
                    + '_iters-{}'.format(args.iters)
                    + '_trial-{}'.format(args.trial)
                    + '.csv')

        # run experiment
        # np.random.shuffle(data_idx) # shuffle datasets
        print('rec:',args.rec,'n_recs:',args.n_recs,'n_init:',args.n_init,
              'iters:',args.iters)
        results = run_experiment(args.rec,
                                 data_idx,
                                 args.n_recs,
                                 args.trial,
                                 knowledge_base,
                                 ml_p,
                                 args.n_init,
                                 args.iters)

        df_results = pd.DataFrame.from_records(results,columns=results[0].keys())
        # write results
        if os.path.exists(out_file):
            with open(out_file, 'a') as out:
                df_results.to_csv(out,index=False,header=False)
        else:
            df_results.to_csv(out_file,index=False)

        print('done. results written to ', out_file)


import random

#draw is the hangman drawing
draw = []
f = open("pendu.txt", "r")
i = 0
for x in f:
    draw.append(x)
    i = i + 1
f.close()
#Dict is the word dictionary (here a list)
dict = []
d = open("dict.txt", "r")
i = 0
for x in d:
    dict.append(x)
    i = i + 1
d.close()

#Choosing the word
word = random.randrange(0, len(dict))
wordlen = len(dict[word]) - 1
word = dict[word]
hideword = ["_ "] * wordlen
print(hideword)

step = 0
fail = 0
win = False
lettre = "00"
#Game loop
while fail < 8 and win == False:
    #choose letter
    while lettre.isalpha() == False or len(lettre) > 1 :
        lettre = input("Choose a letter : ")
        if len(lettre) > 1 :
            print("One letter at the time please")
        if lettre.isalpha() == False :
            print("Sorry, I take only letter")
    lettre = lettre.upper()
    #Check if letter is in the word
    if lettre in word :
        for x in range(len(word)):
            if word[x] == lettre:
                hideword[x] = lettre
    else :
        while ";" not in draw[step] and step + 1 < len(draw):
            print(draw[step])
            step = step + 1
        step = step +1
        fail = fail +1
    print(hideword)
    #check if you win
    if "_ " in hideword:
        win = False
    else :
        win = True
    lettre = "00"

if win == True:
    print("Congratulations, you win")
else :
    print("Too bad, you lose")


def Hirei(x):
    y = 3 * x
    return y
    #ここで比例の関数（function）は終わり。

YnoAtai = Hirei(3)
print("y=" + str(YnoAtai))

#for x in range(0, 100):
#    y = Hirei(a, x)
#    print(y)


#!/usr/bin/env python

""" A unittest script for the WgsAssembledSeqSet module. """

import unittest
import json
import tempfile

from cutlass import WgsAssembledSeqSet

from CutlassTestConfig import CutlassTestConfig
from CutlassTestUtil import CutlassTestUtil

# pylint: disable=W0703, C1801

class WgsAssembledSeqSetTest(unittest.TestCase):
    """ A unit test class for the WgsAssembledSeqSet class. """

    session = None
    util = None

    @classmethod
    def setUpClass(cls):
        """ Setup for the unittest. """
        # Establish the session for each test method
        cls.session = CutlassTestConfig.get_session()
        cls.util = CutlassTestUtil()

    def testImport(self):
        """ Test the importation of the WgsAssembledSeqSet module. """
        success = False
        try:
            from cutlass import WgsAssembledSeqSet
            success = True
        except Exception:
            pass

        self.failUnless(success)
        self.failIf(WgsAssembledSeqSet is None)

    def testSessionCreate(self):
        """ Test the creation of a WgsAssembledSeqSet via the session. """
        success = False
        seq_set = None

        try:
            seq_set = self.session.create_object("wgs_assembled_seq_set")

            success = True
        except Exception:
            pass

        self.failUnless(success)
        self.failIf(seq_set is None)

    def testPrivateFiles(self):
        """ Test the private_files property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.boolTypeTest(self, seq_set, "private_files")

        self.util.boolPropertyTest(self, seq_set, "private_files")

    def testToJson(self):
        """ Test the generation of JSON from a WgsAssembledSeqSet instance. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")
        success = False

        comment = "Test comment"
        private_files = False

        seq_set.comment = comment
        seq_set.private_files = private_files

        seqset_json = None

        try:
            seqset_json = seq_set.to_json()
            success = True
        except Exception:
            pass

        self.assertTrue(success, "Able to use 'to_json'.")
        self.assertTrue(seqset_json is not None, "to_json() returned data.")

        parse_success = False

        try:
            seqset_data = json.loads(seqset_json)
            parse_success = True
        except Exception:
            pass

        self.assertTrue(parse_success, "to_json() did not throw an exception.")
        self.assertTrue(seqset_data is not None, "to_json() returned parsable JSON.")

        self.assertTrue('meta' in seqset_data, "JSON has 'meta' key in it.")

        self.assertEqual(seqset_data['meta']['comment'],
                         comment, "'comment' in JSON had expected value.")

        self.assertEqual(seqset_data['meta']['private_files'],
                         private_files,
                         "'private_files' in JSON had expected value."
                        )

    def testId(self):
        """ Test the id property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.assertTrue(seq_set.id is None,
                        "New template WgsAssembledSeqSet has no ID.")

        with self.assertRaises(AttributeError):
            seq_set.id = "test"

    def testVersion(self):
        """ Test the version property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.assertTrue(seq_set.version is None,
                        "New template WgsAssembledSeqSet has no version.")

        with self.assertRaises(ValueError):
            seq_set.version = "test"

    def testAssembler(self):
        """ Test the assembler property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.stringTypeTest(self, seq_set, "assembler")

        self.util.stringPropertyTest(self, seq_set, "assembler")

    def testAssemblyName(self):
        """ Test the assembly_name property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.stringTypeTest(self, seq_set, "assembly_name")

        self.util.stringPropertyTest(self, seq_set, "assembly_name")

    def testChecksumsLegal(self):
        """ Test the checksums property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")
        success = False
        checksums = {"md5": "asdf32qrfrae"}

        try:
            seq_set.checksums = checksums
            success = True
        except Exception:
            pass

        self.assertTrue(success, "Able to use the 'checksums' setter.")

        self.assertEqual(seq_set.checksums['md5'],
                         checksums['md5'],
                         "Property getter for 'checksums' works.")

    def testComment(self):
        """ Test the comment property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.stringTypeTest(self, seq_set, "comment")

        self.util.stringPropertyTest(self, seq_set, "comment")

    def testIllegalFormat(self):
        """ Test the format property with an illegal value. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        with self.assertRaises(Exception):
            seq_set.format = 1

    def testLegalFormat(self):
        """ Test the format property with a legal value. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")
        seq_set = self.session.create_object("wgs_assembled_seq_set")
        success = False
        format_ = "fasta"

        try:
            seq_set.format = format_
            success = True
        except Exception:
            pass

        self.assertTrue(success, "Able to use the 'format' setter")

        self.assertEqual(seq_set.format, format_,
                         "Property getter for 'format' works.")

    def testFormatDoc(self):
        """ Test the format_doc property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.stringTypeTest(self, seq_set, "format_doc")

        self.util.stringPropertyTest(self, seq_set, "format_doc")

    def testIllegalSequenceType(self):
        """ Test the sequence_type property with an illegal value. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        # Test int argument
        with self.assertRaises(Exception):
            seq_set.sequence_type = 1

        # Test list argument
        with self.assertRaises(Exception):
            seq_set.sequence_type = ['a', 'b', 'c']

        # Test dict argument
        with self.assertRaises(Exception):
            seq_set.sequence_type = {'a': 1, 'b': 2}

    def testLegalSequenceType(self):
        """ Test the sequence_type property with a legal value. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")
        success = False
        sequence_type = "nucleotide"

        try:
            seq_set.sequence_type = sequence_type
            success = True
        except Exception:
            pass

        self.assertTrue(success, "Able to use the 'sequence_type' setter.")

        self.assertEqual(seq_set.sequence_type, sequence_type,
                         "Property getter for 'sequence_type' works.")

    def testSize(self):
        """ Test the size property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        self.util.intTypeTest(self, seq_set, "size")

        self.util.intPropertyTest(self, seq_set, "size")

    def testTags(self):
        """ Test the tags property. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        tags = seq_set.tags
        self.assertTrue(type(tags) == list, "tags() method returns a list.")
        self.assertEqual(len(tags), 0, "Template object 'tags' list is empty.")

        new_tags = ["tagA", "tagB"]

        seq_set.tags = new_tags
        self.assertEqual(seq_set.tags, new_tags,
                         "Can set tags on a WgsAssembledSeqSet.")

        json_str = seq_set.to_json()
        doc = json.loads(json_str)
        self.assertTrue('tags' in doc['meta'],
                        "JSON representation has 'tags' field in 'meta'.")

        self.assertEqual(doc['meta']['tags'], new_tags,
                         "JSON representation had correct tags after setter.")

    def testAddTag(self):
        """ Test the add_tag() method. """
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        seq_set.add_tag("test")
        self.assertEqual(seq_set.tags, ["test"],
                         "Can add a tag to a WgsAssembledSeqSet.")

        json_str = seq_set.to_json()
        doc = json.loads(json_str)

        self.assertEqual(doc['meta']['tags'], ["test"],
                         "JSON representation had correct tags after add_tag().")

        # Try adding the same tag yet again, shouldn't get a duplicate
        with self.assertRaises(ValueError):
            seq_set.add_tag("test")

        json_str = seq_set.to_json()
        doc2 = json.loads(json_str)

        self.assertEqual(doc2['meta']['tags'], ["test"],
                         "JSON document did not end up with duplicate tags.")

    def testRequiredFields(self):
        """ Test the required_fields() static method. """
        required = WgsAssembledSeqSet.required_fields()

        self.assertEqual(type(required), tuple,
                         "required_fields() returns a tuple.")

        self.assertTrue(len(required) > 0,
                        "required_field() did not return empty value.")

    def testLoadSaveDeleteWgsAssembledSeqSet(self):
        """ Extensive test for the load, edit, save and delete functions. """
        temp_file = tempfile.NamedTemporaryFile(delete=False).name

        # attempt to save the WgsAssembledSeqSet at all points before and
        # after adding the required fields
        seq_set = self.session.create_object("wgs_assembled_seq_set")

        assembler = "Test assembler"
        assembly_name = "Test assembly name"
        checksums = {"md5": "68b329da9893e34099c7d8ad5cb9c940"}
        comment = "Test comment"
        format_ = "fasta"
        format_doc = "http://example.com"
        sequence_type = "nucleotide"
        size = 20000
        study = "prediabetes"
        links = {"computed_from":[]}

        self.assertFalse(seq_set.save(),
                         "WgsAssembledSeqSet not saved successfully, " + \
                         "no required fields.")

        seq_set.comment = comment

        # Setting just the comment should not be sufficient. Let's verify.
        self.assertFalse(seq_set.save(),
                         "WgsAssembledSeqSet not saved successfully.")

        seq_set.links = links

        self.assertFalse(seq_set.save(),
                         "WgsAssembledSeqSet not saved successfully.")

        seq_set.assembler = assembler
        seq_set.assembly_name = assembly_name
        seq_set.checksums = checksums
        seq_set.format = format_
        seq_set.format_doc = format_doc
        seq_set.sequence_type = sequence_type
        seq_set.size = size
        seq_set.study = study
        seq_set.local_file = temp_file

        # make sure seq_set does not delete if it does not exist
        with self.assertRaises(Exception):
            seq_set.delete()

        self.assertTrue(seq_set.save() is True,
                        "WgsAssembledSeqSet was not saved successfully.")

        # load the WgsAssembledSeqSet that was just saved from the OSDF instance
        seq_set_loaded = self.session.create_object("wgs_assembled_seq_set")
        seq_set_loaded = seq_set_loaded.load(seq_set.id)

        # check all fields were saved and loaded successfully
        self.assertEqual(seq_set.comment,
                         seq_set_loaded.comment,
                         "WgsAssembledSeqSet comment not saved & loaded successfully.")

        # WgsAssembledSeqSet is deleted successfully
        self.assertTrue(seq_set.delete(),
                        "WgsAssembledSeqSet was not deleted successfully.")

        # the WgsAssembledSeqSet of the initial ID should not load successfully
        load_test = self.session.create_object("wgs_assembled_seq_set")
        with self.assertRaises(Exception):
            load_test = load_test.load(seq_set.id)

if __name__ == '__main__':
    unittest.main()


#!/usr/bin/env python
import rospy
import gazebo_msgs.msg
import random
import tf
import numpy as np
import time
import csv
from gazebo_msgs.msg import ModelStates
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Twist
from gazebo_msgs.srv import SetModelState
from turtlebot3_waypoint_navigation.srv import PheroReset, PheroResetResponse
from math import *
from time import sleep

from std_msgs.msg import Float32MultiArray

class WaypointNavigation:

    MAX_FORWARD_SPEED = 0.5
    MAX_ROTATION_SPEED = 0.5
    cmdmsg = Twist()
    index = 0

    # Tunable parameters
    wGain = 10
    vConst = 0.5
    distThr = 0.2
    pheroThr = 1

    def __init__(self):

        self.num_robots = 1
        self.num_experiments = 20

        # Initialise pheromone values
        self.phero = [0.0] * 9
        self.phero_sum = 0.0

        # Initialise speed
        self.move_cmd = Twist()

        # Initialise positions
        self.goal = [4,0]
        self.obstacle = [[2,0], [-2,0], [0,2], [0,-2]]

        # Initialise ros related topics
        self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 1)
        self.sub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.Callback, (self.pub, self.cmdmsg, self.goal))
        #self.sub_test = rospy.Subscriber('/gazebo/model_states', ModelStates, self.testCallback)
        self.sub_phero = rospy.Subscriber('/phero_value', Float32MultiArray, self.ReadPhero)
        self.rate = rospy.Rate(300)

        self.target_x = 4
        self.target_y = 0

        self.prev_x = 0.0
        self.prev_y = 0.0

        # Initialise parameters

        self.step_size = 0.1
        #self.b_range = np.arange(0, 1+self.step_size, self.step_size)
        self.v_range = np.arange(0.2, 1+self.step_size, self.step_size)
        self.w_range = np.arange(0.2, 1+self.step_size, self.step_size)

        self.BIAS = 0.25
        self.V_COEF = 1.0#self.v_range[0]
        self.W_COEF = 0.2#self.w_range[0]

        #self.b_size = self.b_range.size
        self.v_size = self.v_range.size
        self.w_size = self.w_range.size

        self.b_counter = 0
        self.v_counter = 0
        self.w_counter = 0

        # Initialise simulation
        self.counter_step = 0
        self.counter_collision = 0
        self.counter_success = 0
        self.arrival_time = []
        self.target_index = 0
        self.radius = 4

        # Flags
        self.is_collided = False
        self.is_goal = False
        self.is_timeout = False

        # File name
        self.time_str = time.strftime("%Y%m%d-%H%M%S")
        self.file_name = "manual_{}_{}".format(self.num_robots, self.time_str)
        print(self.file_name)

        # Initialise simulation
        self.reset_timer = time.time()
        self.reset()
        self.reset_flag = False

    def ReadPhero(self, message):
        phero_data = message.data
        self.phero = phero_data
        self.phero_sum = np.sum(np.asarray(phero_data))

    def Callback(self, message, cargs):

        '''
        Main Function
        - Receive position data
        - Generate action
        '''
        # ========================================================================= #
	    #                           Initialisation                                  #
	    # ========================================================================= #

        pub, msg, goal = cargs
        goal = self.goal

        for i in range(len(message.name)):
            if message.name[i] == 'turtlebot3_waffle_pi':
                tb3 = i
            if message.name[i] == 'unit_sphere_0_0':
                tg = i
        pose = message.pose[tb3]
        twist = message.twist[tb3]

        pos = pose.position
        ori = pose.orientation
        angles = tf.transformations.euler_from_quaternion((ori.x, ori.y, ori.z, ori.w))

        theta = angles[2]

        # P controller
        v = 0
        w = 0

        # Index for # of goals
        index = self.index
        distance = sqrt((pos.x-self.target_x)**2+(pos.y-self.target_y)**2)

        # Reset condition reset (to prevent unwanted reset due to delay of position message subscription)
        step_timer = time.time()
        reset_time = step_timer - self.reset_timer

        # ========================================================================= #
	    #                          Action & State assignment                        #
	    # ========================================================================= #

        if (self.phero_sum > self.pheroThr):
            msg = self.PheroOA(self.phero)
            v = msg.linear.x
            w = msg.angular.z
        # Adjust velocities
        elif (distance > self.distThr):
            v = self.vConst
            yaw = atan2(self.target_y-pos.y, self.target_x-pos.x)
            u = yaw - theta
            bound = atan2(sin(u), cos(u))
            w = min(1.0, max(-1.0, self.wGain*bound))
            msg.linear.x = v
            msg.angular.z = w
            self.reset_flag = False
        elif (distance <= self.distThr and reset_time > 1):
            msg = Twist()
            self.is_goal = True
            self.reset()
        distance_to_obs = [1.0]*len(self.obstacle)
        for i in range(len(distance_to_obs)):
            distance_to_obs[i] = sqrt((pos.x-self.obstacle[i][0])**2+(pos.y-self.obstacle[i][1])**2)
        if (distance_to_obs[0] < 0.3 or distance_to_obs[1] < 0.3 or distance_to_obs[2] < 0.3 or distance_to_obs[3] < 0.3) and reset_time > 1:
            msg = Twist()
            self.is_collided = True
            self.reset()

        if reset_time > 40.0:
            print("Times up!")
            self.is_timeout = True
            self.reset()

        # Publish velocity
        self.pub.publish(msg)

        self.prev_x = pos.x
        self.prev_y = pos.y

        # Reporting
        #print("Distance to goal {}".format(distance))
        #print('Callback: x=%2.2f, y=%2.2f, dist=%4.2f, cmd.v=%2.2f, cmd.w=%2.2f' %(pos.x,pos.y,distance,v,w))

    # Angular velocity coefficient (When avg phero is high, it is more sensitive)
    def velCoef(self, value1, value2):
        '''
        - val_avg (0, 1)
        - val_dif (-1, 1)
        - dif_coef (1, 2.714)
        - coefficient (-2.714, 2.714)
        '''
        val_avg = (value1 + value2)/2
        val_dif = value1 - value2
        dif_coef = exp(val_avg)

        return dif_coef*val_dif

    def PheroOA(self, phero):
        '''
        Pheromone-based obstacle avoidance algorithm
        - Input: 9 cells of pheromone
        - Output: Twist() to avoid obstacle
        '''
        # Constants:
        # Constants:
        BIAS = self.BIAS
        V_COEF = self.V_COEF
        W_COEF = self.W_COEF
        #BIAS = 0.25
        #V_COEF = 0.2
        #W_COEF = 0.3

        # Initialise values
        avg_phero = np.average(np.asarray(phero)) # values are assigned from the top left (135 deg) to the bottom right (-45 deg) ((0,1,2),(3,4,5),(6,7,8))
        unit_vecs = np.asarray([[1,0], [sqrt(2)/2, sqrt(2)/2], [0,1], [-sqrt(2)/2, sqrt(2)/2]])
        vec_coefs = [0.0] * 4
        twist = Twist()

        # Calculate vector weights
        vec_coefs[0] = self.velCoef(phero[5], phero[3])
        vec_coefs[1] = self.velCoef(phero[2], phero[6])
        vec_coefs[2] = self.velCoef(phero[1], phero[7])
        vec_coefs[3] = self.velCoef(phero[0], phero[8])
        vec_coefs = np.asarray(vec_coefs).reshape(4,1)
        vel_vecs = np.multiply(unit_vecs, vec_coefs)
        vel_vec = np.sum(vel_vecs, axis=0)

        ang_vel = W_COEF*atan2(vel_vec[1], vel_vec[0])

        # Velocity assignment
        twist.linear.x = BIAS + V_COEF*avg_phero
        twist.angular.z = ang_vel

        return twist

    def reset(self):

        '''
        Resettng the Experiment
        1. Update the counter based on the flag from step
        2. Assign next positions and reset
        3. Log the result in every selected time-step
        '''

        # ========================================================================= #
	    #                           COUNTER UPDATE                                  #
	    # ========================================================================= #

        # Increment Collision Counter
        if self.is_collided == True:
            print("Collision!")
            self.counter_collision += 1
            self.counter_step += 1

        # Increment Arrival Counter and store the arrival time
        if self.is_goal == True:
            print("Arrived goal!")
            self.counter_success += 1
            self.counter_step += 1
            arrived_timer = time.time()
            art = arrived_timer-self.reset_timer
            self.arrival_time.append(art)
            print("Episode time: %0.2f"%art)

        if self.is_timeout == True:
            self.counter_collision += 1
            self.counter_step += 1
            print("Timeout!")

        # Reset the flags
        self.is_collided = False
        self.is_goal = False
        self.is_timeout = False

        # ========================================================================= #
	    #                                  RESET                                    #
	    # ========================================================================= #

        angle_target = self.target_index*2*pi/self.num_experiments

        self.target_x = self.radius*cos(angle_target)
        self.target_y = self.radius*sin(angle_target)

        if self.target_index < self.num_experiments:
            self.target_index += 1
        else:
            self.target_index = 0

        self.is_collided = False

        # Reset Turtlebot position
        state_msg = ModelState()
        state_msg.model_name = 'turtlebot3_waffle_pi'
        state_msg.pose.position.x = 0.0
        state_msg.pose.position.y = 0.0
        state_msg.pose.position.z = 0.0
        state_msg.pose.orientation.x = 0
        state_msg.pose.orientation.y = 0
        state_msg.pose.orientation.z = 0
        state_msg.pose.orientation.w = 0

        # Reset Target Position
        state_target_msg = ModelState()
        state_target_msg.model_name = 'unit_sphere_0_0' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
        state_target_msg.pose.position.x = self.target_x
        state_target_msg.pose.position.y = self.target_y
        state_target_msg.pose.position.z = 0.0
        state_target_msg.pose.orientation.x = 0
        state_target_msg.pose.orientation.y = 0
        state_target_msg.pose.orientation.z = 0
        state_target_msg.pose.orientation.w = 0

        rospy.wait_for_service('gazebo/reset_simulation')

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
            resp = set_state(state_msg)
            resp_targ = set_state(state_target_msg)
        except rospy.ServiceException as e:
            print("Service Call Failed: %s"%e)

        self.move_cmd.linear.x = 0.0
        self.move_cmd.angular.z = 0.0
        self.pub.publish(self.move_cmd)
        self.pub.publish(self.move_cmd)

        rospy.wait_for_service('phero_reset')
        try:
            phero_reset = rospy.ServiceProxy('phero_reset', PheroReset)
            resp = phero_reset(True)
            print("Reset Pheromone grid successfully: {}".format(resp))
        except rospy.ServiceException as e:
            print("Service Failed %s"%e)

        # ========================================================================= #
	    #                                  LOGGING                                  #
	    # ========================================================================= #

        if self.counter_step == 0:
            with open('/home/swn/catkin_ws/src/turtlebot3_waypoint_navigation/src/log/csv/{}.csv'.format(self.file_name), mode='w') as csv_file:
                csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
                csv_writer.writerow(['Episode', 'Bias', 'Vcoef', 'Wcoef', 'Success Rate', 'Average Arrival time', 'Standard Deviation'])

        if self.counter_step != 0:
            if (self.counter_collision != 0 and self.counter_success != 0):
                succ_percentage = 100*self.counter_success/(self.counter_success+self.counter_collision)
            else:
                succ_percentage = 0
            print("Counter: {}".format(self.counter_step))

        if (self.counter_step % 10 == 0 and self.counter_step != 0):
            print("BIAS: {}, V_COEF: {}, W_COEF: {}".format(self.BIAS, self.V_COEF, self.W_COEF))
            print("Success Rate: {}%".format(succ_percentage))

        if (self.counter_step % 20 == 0 and self.counter_step != 0):
            avg_comp = np.average(np.asarray(self.arrival_time))
            std_comp = np.std(np.asarray(self.arrival_time))
            print("{} trials ended. Success rate: {}, average completion time: {}, Standard deviation: {}".format(self.counter_step, succ_percentage, avg_comp, std_comp))

            with open('/home/swn/catkin_ws/src/turtlebot3_waypoint_navigation/src/log/csv/{}.csv'.format(self.file_name), mode='a') as csv_file:
                csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
                csv_writer.writerow(['%i'%self.counter_step, '%0.2f'%self.BIAS, '%0.2f'%self.V_COEF, '%0.2f'%self.W_COEF, '%0.2f'%succ_percentage, '%0.2f'%avg_comp, '%0.2f'%std_comp])

            self.paramUpdate()
            self.arrival_time = []
            self.counter_collision = 0
            self.counter_success = 0
            self.target_index = 0

        self.reset_timer = time.time()
        self.reset_flag = True

    def paramUpdate(self):
        '''
        Parameter update after the number of experiments for a parameter set finished
        '''
        print("Parameters are updated!")
        if (self.w_counter < self.w_size-1):
            self.w_counter += 1
            self.W_COEF = self.w_range[self.w_counter]
        elif (self.v_counter < self.v_size-1):
            self.w_counter = 0
            self.v_counter += 1
            self.W_COEF = self.w_range[self.w_counter]
            self.V_COEF = self.v_range[self.v_counter]
        else:
            print("Finish Iteration of parameters")
            sys.exit()

if __name__ == '__main__':
    rospy.init_node('pose_reading')
    wayN = WaypointNavigation()
    rospy.spin()


from django.db.models.signals import post_save
from django.dispatch import receiver
from .models import Transaction
from .jobs.sanctionjob import SanctionJob
from .jobs.pepjob import PEPJob
from .jobs.assessmentjob import AssessmentJob

"""
Signals gets triggered when there are
any saves to the database.
"""

@receiver(post_save, sender=Transaction)
def process_transaction(sender, instance, created, **kwarks):
    if created:
        # Lets kick off the first process
        print("POST SAVE CREATED")
        sanctionJob = SanctionJob(transaction=instance)
        sanctionJob.run()
    else:
        # Check where we are in the process
        print ("POST SAVE UPDATED {} {}".format(instance.id, instance.status))
        if instance.status == Transaction.Status.PEPCHECK:
            print('Starting PEP job')
            pepjob = PEPJob(transaction=instance)
            pepjob.run()
        elif instance.status == Transaction.Status.ASSESSMENT:
            print('Starting Assessment job')
            assessmentjob = AssessmentJob(transaction=instance)
            assessmentjob.run()


"""
This file is needed only for future reference to
load data from wiki data.
DOwnload link : http://people.apache.org/~mikemccand/enwiki-20120502-lines-1k.txt.lzma
"""
import glob
import itertools
import os
import random

from couchbase.bucket import Bucket

class Docgen(object):
    def __init__(self, location, cb_url, database, chunk_size=1024):
        self.chunk_size = chunk_size
        self.location = location
        self.cb_url = cb_url
        self.database = database
        self.counts = 0
        '''
        The keygen part to create keys to optimize space
        '''
        self.key1 = self.keygen(2)
        self.key2 = self.keygen(4)
        self.key3 = self.keygen(5)

    @staticmethod
    def keygen(self, keynum):
        '''
        the ranges includes all ascii characters from 47 - 123

        '''
        s = list(map(chr, range(47, 123)))
        random.shuffle(s)
        iters = itertools.permutations(s, keynum)
        return iters

    def read_in_chunks(self, file_object):
        """Lazy function (generator) to read a file piece by piece.
        Default chunk size: 1k."""
        while True:
            data = file_object.read(self.chunk_size)
            if not data:
                break
            yield data

    def insert_cb(self, cb, f):
        for piece in self.read_in_chunks(f):
            test = {}
            piece = piece.replace('\n', ' ')
            test['text'] = piece
            key = ""
            if self.counts < 1000:
                key = self.key1.next()
            elif self.counts < 100000:
                key = self.key2.next()
            else:
                key = self.key3.next()
            key = ''.join(key)
            cb.upsert(key, test)

            if self.counts == 1000000:
                return
            self.counts += 1

    def start_load(self):
        os.chdir(self.location)
        c = Bucket("couchbase://{}/{}?operation_timeout=10".format(self.cb_url, self.database))
        for file in glob.glob("*.txt"):
            try:
                f = open(file)
                self.insert_cb(c, f)
                f.close()
            except Exception as e:
                print (self.counts, file)
                raise e

A = Docgen('/data/wikidata', '172.23.123.38', 'bucket-1')
A.start_load()


import tensorflow as tf
import collections
import os
#from PIL import Image
import random
import numpy as np
from datetime import datetime
from sklearn.metrics import confusion_matrix

slim = tf.contrib.slim

global first
first = True

'''
如果激活函数使用sigmoid和tanh，最好使用xavir
tf.contrib.layers.xavier_initializer_conv2d
如果使用relu，则最好使用he initial
tf.contrib.layers.variance_scaling_initializer
'''

classnum=12
batchsize=128
testnum = tf.placeholder(tf.int32)
trainnum = tf.placeholder(tf.int32)
validnum = tf.placeholder(tf.int32)
learnrate = tf.placeholder(tf.float32)

path=r'C:\workspace\Alexnet\model\train_model.ckpt'
trainpath=[r'C:\workspace\Alexnet\tfrecord\train_L.tfrecords',
           r'C:\workspace\Alexnet\tfrecord\train_001_L.tfrecords']
testpath=[r'C:\workspace\Alexnet\tfrecord\test_L.tfrecords',
          r'C:\workspace\Alexnet\tfrecord\test_001_L.tfrecords']
validpath = [r'C:\workspace\Alexnet\tfrecord\validation.tfrecords']

def getinputs(path,preprocess):
    filename_queue=tf.train.string_input_producer(path)
    reader=tf.TFRecordReader()
    _,serialized_example=reader.read(filename_queue)
    features=tf.parse_single_example(serialized_example,
                                     features={
                                        'label':tf.FixedLenFeature([], tf.int64),
                                        'img_raw' : tf.FixedLenFeature([], tf.string),
                                     })
    image=tf.decode_raw(features['img_raw'],tf.uint8)
    label=tf.cast(features['label'],tf.int32)
    image=tf.reshape(image,[64,64,1])
    if preprocess:
        #image=tf.image.random_flip_left_right(image)
        #image=tf.image.random_flip_up_down(image)
        image = tf.cast(image,tf.float32)

        #image = tf.image.random_hue(image, max_delta=0.05)
        image = tf.image.random_contrast(image, lower=0.8, upper=1.2)
        image = tf.image.random_brightness(image, max_delta=0.2)
        #image = tf.image.random_saturation(image, lower=0.0, upper=2.0)
        image = tf.minimum(image, 255.0)
        image = tf.maximum(image, 0.0)
    return image,label

def get_batch(image,label,batch_size,crop_size):
    #print(image.shape)
    #print(label.shape)
    #images,labels=tf.train.batch([image,label],batch_size=batch_size)
    images,labels=tf.train.shuffle_batch([image,label],
        batch_size=batch_size,num_threads=10,capacity=10000,min_after_dequeue=300)
    #images=[tf.image.random_flip_left_right(image,seed=None) for image in images]
    return images,tf.reshape(labels,[batch_size])

def get_test_batch(image,label,batch_size):
    images,labels=tf.train.shuffle_batch([image,label],
        batch_size=batch_size,num_threads=10,capacity=10000,min_after_dequeue=500)
    #images,labels=tf.train.batch([image,label],batch_size=batch_size)
    return images,tf.reshape(labels,[batch_size])

def get_valid_batch(image,label,batch_size):
    images,labels=tf.train.shuffle_batch([image,label],
        batch_size=batch_size,num_threads=10,capacity=10000,min_after_dequeue=500)
    #images,labels=tf.train.batch([image,label],batch_size=batch_size)
    return images,tf.reshape(labels,[batch_size])

class trainwork(object):
    def __init__(self):
        with tf.variable_scope('scop'):
            self.weights={
                'conv1':tf.get_variable('w1', [3,3,1,24],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'conv2':tf.get_variable('w2', [3,3,24,48],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'conv3':tf.get_variable('w3', [3,3,48,56],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'conv4':tf.get_variable('w4', [3,3,56,56],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'conv5':tf.get_variable('w5', [3,3,56,24],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'fc1':tf.get_variable('w6', [8*8*24,1024],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'fc2':tf.get_variable('w7', [1024,512],initializer=tf.contrib.layers.variance_scaling_initializer()),
                'out':tf.get_variable('w8', [512,classnum],initializer=tf.contrib.layers.variance_scaling_initializer())
            }
            self.biases={
                'conv1': tf.get_variable('b1', [24],initializer=tf.constant_initializer(0.1)),
                'conv2': tf.get_variable('b2', [48],initializer=tf.constant_initializer(0.1)),
                'conv3': tf.get_variable('b3', [56],initializer=tf.constant_initializer(0.1)),
                'conv4': tf.get_variable('b4', [56],initializer=tf.constant_initializer(0.1)),
                'conv5': tf.get_variable('b5', [24],initializer=tf.constant_initializer(0.1)),
                'fc1': tf.get_variable('b6', [1024],initializer=tf.constant_initializer(0.1)),
                'fc2': tf.get_variable('b7', [512],initializer=tf.constant_initializer(0.1)),
                'out': tf.get_variable('b8', [classnum],initializer=tf.constant_initializer(0.1))
            }

    def inference(self,images,preprocessed):
        if preprocessed:
            images=images/255.0
        else:
            images=tf.cast(images,tf.float32)/255.0

        conv1=tf.nn.conv2d(images,self.weights['conv1'],strides=[1,1,1,1],padding='SAME')+self.biases['conv1']
        relu1=tf.nn.relu(conv1)
        norm1=tf.nn.local_response_normalization(relu1,2,1.0,1,1)
        pool1=tf.nn.max_pool(norm1,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv2 = tf.nn.conv2d(pool1,self.weights['conv2'],strides=[1,1,1,1],padding='SAME')+self.biases['conv2']
        relu2=tf.nn.relu(conv2)
        norm2=tf.nn.local_response_normalization(relu2,2,1.0,1,1)
        pool2=tf.nn.max_pool(norm2,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv3=tf.nn.conv2d(pool2,self.weights['conv3'],strides=[1,1,1,1],padding='SAME')+self.biases['conv3']
        relu3=tf.nn.relu(conv3)

        conv4=tf.nn.conv2d(relu3,self.weights['conv4'],strides=[1,1,1,1],padding='SAME')+self.biases['conv4']
        relu4=tf.nn.relu(conv4)

        conv5=tf.nn.conv2d(relu4,self.weights['conv5'],strides=[1,1,1,1],padding='SAME')+self.biases['conv5']
        relu5=tf.nn.relu(conv5)
        pool5=tf.nn.max_pool(relu5,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        flat=tf.reshape(pool5,[-1,8*8*24])
        fc1=tf.matmul(flat, self.weights['fc1'])+self.biases['fc1']
        fc1_relu=tf.nn.relu(fc1)
        fc1_drop=tf.nn.dropout(fc1_relu,0.8)#dropout也可以放在其它层

        fc2=tf.matmul(fc1_drop, self.weights['fc2'])+self.biases['fc2']
        fc2_relu=tf.nn.relu(fc2)
        fc2_drop=tf.nn.dropout(fc2_relu,0.8)
        out=tf.matmul(fc2_drop,self.weights['out'])+self.biases['out']
        return out

    def test_inference(self,images):
        images=tf.cast(images,tf.float32)/255.0
        conv1=tf.nn.conv2d(images,self.weights['conv1'],strides=[1,1,1,1],padding='SAME')+self.biases['conv1']
        relu1=tf.nn.relu(conv1)
        norm1=tf.nn.local_response_normalization(relu1,2,1.0,1,1)
        pool1=tf.nn.max_pool(norm1,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv2 = tf.nn.conv2d(pool1,self.weights['conv2'],strides=[1,1,1,1],padding='SAME')+self.biases['conv2']
        relu2=tf.nn.relu(conv2)
        norm2=tf.nn.local_response_normalization(relu2,2,1.0,1,1)
        pool2=tf.nn.max_pool(norm2,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv3=tf.nn.conv2d(pool2,self.weights['conv3'],strides=[1,1,1,1],padding='SAME')+self.biases['conv3']
        relu3=tf.nn.relu(conv3)

        conv4=tf.nn.conv2d(relu3,self.weights['conv4'],strides=[1,1,1,1],padding='SAME')+self.biases['conv4']
        relu4=tf.nn.relu(conv4)

        conv5=tf.nn.conv2d(relu4,self.weights['conv5'],strides=[1,1,1,1],padding='SAME')+self.biases['conv5']
        relu5=tf.nn.relu(conv5)
        pool5=tf.nn.max_pool(relu5,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        flat=tf.reshape(pool5,[-1,8*8*24])
        fc1=tf.matmul(flat, self.weights['fc1'])+self.biases['fc1']
        fc1_relu=tf.nn.relu(fc1)
        #fc1_drop=tf.nn.dropout(fc1_relu,0.8)

        fc2=tf.matmul(fc1_relu, self.weights['fc2'])+self.biases['fc2']
        fc2_relu=tf.nn.relu(fc2)
        #fc2_drop=tf.nn.dropout(fc2_relu,0.8)
        out=tf.matmul(fc2_relu,self.weights['out'])+self.biases['out']
        return out

    def valid_inference(self,images):
        images=tf.cast(images,tf.float32)/255.0
        conv1=tf.nn.conv2d(images,self.weights['conv1'],strides=[1,1,1,1],padding='SAME')+self.biases['conv1']
        relu1=tf.nn.relu(conv1)
        norm1=tf.nn.local_response_normalization(relu1,2,1.0,1,1)
        pool1=tf.nn.max_pool(norm1,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv2 = tf.nn.conv2d(pool1,self.weights['conv2'],strides=[1,1,1,1],padding='SAME')+self.biases['conv2']
        relu2=tf.nn.relu(conv2)
        norm2=tf.nn.local_response_normalization(relu2,2,1.0,1,1)
        pool2=tf.nn.max_pool(norm2,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        conv3=tf.nn.conv2d(pool2,self.weights['conv3'],strides=[1,1,1,1],padding='SAME')+self.biases['conv3']
        relu3=tf.nn.relu(conv3)

        conv4=tf.nn.conv2d(relu3,self.weights['conv4'],strides=[1,1,1,1],padding='SAME')+self.biases['conv4']
        relu4=tf.nn.relu(conv4)

        conv5=tf.nn.conv2d(relu4,self.weights['conv5'],strides=[1,1,1,1],padding='SAME')+self.biases['conv5']
        relu5=tf.nn.relu(conv5)
        pool5=tf.nn.max_pool(relu5,ksize=[1,2,2,1],strides=[1,2,2,1],padding='SAME')

        flat=tf.reshape(pool5,[-1,8*8*24])
        fc1=tf.matmul(flat, self.weights['fc1'])+self.biases['fc1']
        fc1_relu=tf.nn.relu(fc1)
        #fc1_drop=tf.nn.dropout(fc1_relu,0.8)

        fc2=tf.matmul(fc1_relu, self.weights['fc2'])+self.biases['fc2']
        fc2_relu=tf.nn.relu(fc2)
        #fc2_drop=tf.nn.dropout(fc2_relu,0.8)
        out=tf.matmul(fc2_relu,self.weights['out'])+self.biases['out']
        return out

    def softmax_loss(self,predicts,labels):

        predicts=tf.nn.softmax(predicts)
        labels=tf.one_hot(labels,classnum)
        #loss=-tf.reduce_sum(labels*tf.log(predicts))
        loss=-tf.reduce_sum(labels*tf.log(tf.clip_by_value(predicts,1e-10,1.0)))

        #loss=tf.reduce_sum(tf.nn.softmax_cross_entropy_with_logits(predicts, labels))
        return loss

    def optimer(self,loss,lr=0.001):

        train_step=tf.train.AdamOptimizer(lr).minimize(loss)
        #train_step=tf.train.GradientDescentOptimizer(lr).minimize(loss)
        return train_step

pre=True
#衰减学习率 每一百步在原来数值上乘0.96
#可以将global_step这个值和Learn rate训练完成后存在本地文件，下次训练加载初始化
#global_step=tf.placeholder('int32')
#learnrate = tf.train.exponential_decay(0.001, global_step, 100, 0.96, staircase=True)

image,label=getinputs(trainpath,pre)
batch_image,batch_label=get_batch(image,label,batchsize,0)
work=trainwork()
inf=work.inference(batch_image,pre)
loss=work.softmax_loss(inf,batch_label)
opti=work.optimer(loss,learnrate)

test_image,test_label=getinputs(testpath,False)
test_image_batch,test_label_batch=get_test_batch(test_image,test_label,testnum)
test_inf=work.test_inference(test_image_batch)
test_labels=tf.one_hot(test_label_batch,classnum)
#train_step=tf.train.GradientDescentOptimizer(0.001).minimize(cross_entropy)
correct_prediction=tf.equal(tf.argmax(test_inf,1),tf.argmax(test_labels,1))
accuracy=tf.reduce_mean(tf.cast(correct_prediction,tf.float32))
test_pre = tf.reshape(test_inf, [testnum, classnum])
test_pre = tf.argmax(test_pre, 1)
test_true = tf.argmax(test_labels, 1)

valid_image,valid_label=getinputs(validpath,False)
valid_image_batch,valid_label_batch=get_valid_batch(valid_image,valid_label,validnum)
valid_inf=work.test_inference(valid_image_batch)
valid_labels=tf.one_hot(valid_label_batch,classnum)
#train_step=tf.train.GradientDescentOptimizer(0.001).minimize(cross_entropy)
valid_correct_prediction=tf.equal(tf.argmax(valid_inf,1),tf.argmax(valid_labels,1))
valid_accuracy=tf.reduce_mean(tf.cast(correct_prediction,tf.float32))
valid_pre = tf.reshape(valid_inf, [validnum, classnum])
valid_pre = tf.argmax(valid_pre, 1)
valid_true = tf.argmax(valid_labels, 1)
#init=tf.initialize_all_variables()

from sklearn.metrics import classification_report

target_names = ['class sg', 'class bm', 'class wd', 'class wt', 'class wj', 'class wo', 'class ym', 'class shq', 'class shj',
                'class no', 'class yh', 'class fb']

init=tf.global_variables_initializer()
config=tf.ConfigProto()
config.gpu_options.allow_growth = True

def train(train_num=64,test_num=32,lr=1e-4,loop_count=10000,report_step=100,save_step=1000,restore=False):
    with tf.Session(config=config) as sess:
        sess.run(init)
        coord = tf.train.Coordinator()
        threads = tf.train.start_queue_runners(coord=coord)
        if restore:
            tf.train.Saver().restore(sess,path)
        feed_dict={
            testnum: test_num,
            trainnum: train_num,
            learnrate:lr
        }
        for i in range(loop_count):
            loss_np, _, label_np, image_np, inf_np = sess.run(
                [loss, opti, batch_label, batch_image, inf],feed_dict=feed_dict)
            if i > 0 and i % report_step == 0:
                accuracy_np = sess.run([accuracy],feed_dict=feed_dict)
                print(i, accuracy_np, loss_np)
            if i > 0 and i % save_step == 0:
                tf.train.Saver().save(sess, path)
        tf.train.Saver().save(sess, path)
        coord.request_stop()
        coord.join(threads)

def test_and_valid(test_loop=1,valid_loop=1,test_num=64,valid_num=64):
    feed_dict={
        testnum: test_num,
        validnum: valid_num
    }
    with tf.Session(config=config) as sess:
        sess.run(init)
        coord = tf.train.Coordinator()
        threads = tf.train.start_queue_runners(coord=coord)
        tf.train.Saver().restore(sess,path)
        #test
        test_acc_avg = 0.0
        test_true_total=np.array([])
        test_pre_total=np.array([])
        for i in range(0, test_loop):

            accuracy_np = sess.run([accuracy],feed_dict=feed_dict)
            test_pre_1, test_true_1 = sess.run([test_pre, test_true],feed_dict=feed_dict)
            test_pre_1 = np.array(test_pre_1)
            test_true_1 = np.array(test_true_1)

            test_acc_avg = test_acc_avg + accuracy_np[0]
            test_true_total = np.concatenate((test_true_total,test_true_1),axis=0)
            test_pre_total = np.concatenate((test_pre_total,test_pre_1), axis=0)
        print('------test_accuracy-----')
        print(test_acc_avg / test_loop)
        print('------test_accuracy-----')

        print('------test_classification_report-----')
        print(classification_report(test_true_total, test_pre_total, target_names=target_names))
        print('------test_classification_report-----')
        print('------test_confusion_matrix-----')
        cm = confusion_matrix(y_true=test_true_total, y_pred=test_pre_total)
        print(cm)
        print('------test_confusion_matrix-----')

        #valid
        if valid_loop > 0:
            valid_acc_avg = 0.0
            valid_true_total=np.array([])
            valid_pre_total=np.array([])
            for i in range(0, valid_loop):
                accuracy_np = sess.run([valid_accuracy],feed_dict=feed_dict)
                valid_pre_1, valid_true_1 = sess.run([valid_pre, valid_true],feed_dict=feed_dict)
                valid_pre_1 = np.array(valid_pre_1)
                valid_true_1 = np.array(valid_true_1)

                valid_acc_avg = valid_acc_avg + accuracy_np[0]
                valid_true_total = np.concatenate((valid_true_total,valid_true_1),axis=0)
                valid_pre_total = np.concatenate((valid_pre_total,valid_pre_1), axis=0)
            print('------valid_accuracy-----')
            print(valid_acc_avg / valid_loop)
            print('------valid_accuracy-----')

            print('------valid_classification_report-----')
            print(classification_report(valid_true_total, valid_pre_total, target_names=target_names))
            print('------valid_classification_report-----')
            print('------valid_confusion_matrix-----')
            cm = confusion_matrix(y_true=valid_true_total, y_pred=valid_pre_total)
            print(cm)
            print('------valid_confusion_matrix-----')

        coord.request_stop()
        coord.join(threads)

def predict_time(loop=100):
    feed_dict={
        testnum:1
    }
    with tf.Session(config=config) as sess:
        sess.run(init)
        coord = tf.train.Coordinator()
        threads = tf.train.start_queue_runners(coord=coord)
        tf.train.Saver().restore(sess,path)
        total=0.0
        for i in range(loop):
            a = datetime.now()
            accuracy_np = sess.run([accuracy],feed_dict=feed_dict)
            b = datetime.now()
            c = (b - a).microseconds
            total+=c
        print('predict_time(ms): ',total/(loop*1000))
        coord.request_stop()
        coord.join(threads)

train(train_num=128,loop_count=1002)
test_and_valid(10,10,200,200)
predict_time(1000)


#!/usr/bin/env python

import numpy as np
import math
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import sys
import time

def workspace_check(x_check, y_check):
    """
        In this function the given value is checked to see if it lies in the workspace

        Parameters
        ----------
        x_check: X coordinate of given input
        y_check: Y coordinate of given input

        Returns
        -------
        True  : If the input lies inside workspace
        False : If the input lies outside workspace
    """

    if (x_check >= 0) and (y_check >= 0) and (x_check <= 10) and (y_check <= 10):
        return True
    else:
        return False

def obstacle_space(x_check, y_check):
    """
        In this function the given value is checked to see if it lies in the obstacle space

        Parameters
        ----------
        x_check: X coordinate of given input
        y_check: Y coordinate of given input

        Returns
        -------
        True  : If the input lies outside obstacle space
        False : If the input lies inside obstacle space
    """

    flag1, flag2, flag3, flag4, flag5 = (True, True, True, True, True)

    # For Circle 1 with center at (2, 2) and radius 1 unit
    if (x_check - 2) ** 2 + (y_check - 2) ** 2 - (1 + clearance) ** 2 < 0:
        flag1 = False

    # For Circle 2 with center at (2, 8) and radius 1 unit
    if (x_check - 2) ** 2 + (y_check - 8) ** 2 - (1 + clearance) ** 2 < 0:
        flag2 = False

    # For Square with center at (1, 5) and dimension 1.5x1.5
    if 0.25 - clearance < x_check < 1.75 + clearance and 4.25 - clearance < y_check < 5.75 + clearance:

        if 0.25 - clearance < x_check < 0.25 and 4.25 - clearance < y_check < 4.25 and \
                (x_check - 0.25) ** 2 + (y_check - 4.25) ** 2 - clearance ** 2 >= 0:
            flag3 = True

        elif 0.25 - clearance < x_check < 0.25 and 5.75 < y_check < 5.75 + clearance and \
                (x_check - 0.25) ** 2 + (y_check - 5.75) ** 2 - clearance ** 2 >= 0:
            flag3 = True

        elif 1.75 < x_check < 1.75 + clearance and 5.75 < y_check < 5.75 + clearance and \
                (x_check - 1.75) ** 2 + (y_check - 5.75) ** 2 - clearance ** 2 >= 0:
            flag3 = True

        elif 1.75 < x_check < 1.75 + clearance and 4.25 - clearance < y_check < 4.25 and \
                (x_check - 1.75) ** 2 + (y_check - 4.25) ** 2 - clearance ** 2 >= 0:
            flag3 = True

        else:
            flag3 = False

    # For Rectangle 1 with center at (5, 5) and dimension 2.5x1.5
    if 3.75 - clearance < x_check < 6.25 + clearance and 4.25 - clearance < y_check < 5.75 + clearance:

        if 3.75 - clearance < x_check < 3.75 and 4.25 - clearance < y_check < 4.25 and \
                (x_check - 3.75) ** 2 + (y_check - 4.25) ** 2 - clearance ** 2 >= 0:
            flag4 = True

        elif 3.75 - clearance < x_check < 3.75 and 5.75 < y_check < 5.75 + clearance and \
                (x_check - 3.75) ** 2 + (y_check - 5.75) ** 2 - clearance ** 2 >= 0:
            flag4 = True

        elif 6.25 < x_check < 6.25 + clearance and 5.75 < y_check < 5.75 + clearance and \
                (x_check - 6.25) ** 2 + (y_check - 5.75) ** 2 - clearance ** 2 >= 0:
            flag4 = True

        elif 6.25 < x_check < 6.25 + clearance and 4.25 - clearance < y_check < 4.25 and \
                (x_check - 6.25) ** 2 + (y_check - 4.25) ** 2 - clearance ** 2 >= 0:
            flag4 = True

        else:
            flag4 = False

    # For Rectangle 2 with center at (8, 3) and dimension 1.5x2.0
    if 7.25 - clearance < x_check < 8.75 + clearance and 2.0 - clearance < y_check < 4.0 + clearance:

        if 7.25 - clearance < x_check < 7.25 and 2.0 - clearance < y_check < 2.0 and \
                (x_check - 7.25) ** 2 + (y_check - 2.0) ** 2 - clearance ** 2 >= 0:
            flag5 = True

        elif 7.25 - clearance < x_check < 7.25 and 4.0 < y_check < 4.0 + clearance and \
                (x_check - 7.25) ** 2 + (y_check - 4.0) ** 2 - clearance ** 2 >= 0:
            flag5 = True

        elif 8.75 < x_check < 8.75 + clearance and 4.0 < y_check < 4.0 + clearance and \
                (x_check - 8.75) ** 2 + (y_check - 4.0) ** 2 - clearance ** 2 >= 0:
            flag5 = True

        elif 8.75 < x_check < 8.75 + clearance and 2.0 - clearance < y_check < 2.0 and \
                (x_check - 8.75) ** 2 + (y_check - 2.0) ** 2 - clearance ** 2 >= 0:
            flag5 = True

        else:
            flag5 = False

    flag = flag1 and flag2 and flag3 and flag4 and flag5

    return flag

def find_index(node_point):
    x_pt, y_pt = node_point

    x_pt_mid = threshold_distance * (0.5 + int(x_pt / threshold_distance))
    y_pt_mid = threshold_distance * (0.5 + int(y_pt / threshold_distance))

    # ----------------------- For x_pt -----------------------

    if x_pt <= x_pt_mid:
        x_temp_ = threshold_distance * int(x_pt / threshold_distance)

    else:
        x_temp_ = threshold_distance * int(1 + x_pt / threshold_distance)

    # ----------------------- For y_pt ----------------

    if y_pt <= y_pt_mid:
        y_temp_ = threshold_distance * int(y_pt / threshold_distance)

    else:
        y_temp_ = threshold_distance * int(1 + y_pt / threshold_distance)

    node_index = np.intersect1d(np.where(np.array(X) == x_temp_), np.where(np.array(Y) == y_temp_))[0]

    return node_index

def a_star(parent_index):
    x_parent, y_parent = node_pt[parent_index]
    parent_angle = node_angle[parent_index]
    parent_cost = node_cost[parent_index]
    iter_bin.append([])  # appending an empty list for each iteration

    # ------------------------------------------ Performing action sets ------------------------------------------------

    actions = [[5, 5], [10, 10], [5, 0], [0, 5], [5, 10], [10, 5]]  # mentioned in class

    for action in actions:
        t = 0
        x_temp = x_parent
        y_temp = y_parent
        theta_temp = parent_angle
        temp_distance = 0

        omega_left, omega_right = action
        temp_x = [x_temp]
        temp_y = [y_temp]

        while t < 1:
            t = t + dt
            x_temp0 = x_temp
            y_temp0 = y_temp
            theta_temp0 = theta_temp % (2 * math.pi)

            dx_temp = 0.5 * r_wheel * (omega_left + omega_right) * math.cos(theta_temp) * dt
            dy_temp = 0.5 * r_wheel * (omega_left + omega_right) * math.sin(theta_temp) * dt
            dtheta_temp = (r_wheel / L_wheel) * (omega_right - omega_left) * dt

            x_temp = x_temp0 + dx_temp
            y_temp = y_temp0 + dy_temp
            theta_temp = (theta_temp0 + dtheta_temp) % (2 * math.pi)

            temp_x.append(x_temp)
            temp_y.append(y_temp)
            temp_distance += math.sqrt(dx_temp ** 2 + dy_temp ** 2)

        is_valid = workspace_check(temp_x[-1], temp_y[-1]) and obstacle_space(temp_x[-1], temp_y[-1])

        if is_valid:

            neighbor_pt = np.array([temp_x[-1], temp_y[-1]])
            neighbor_index = find_index(neighbor_pt)

            if neighbor_index not in visited_nodes_index:

                if node_cost[neighbor_index] > parent_cost + temp_distance:
                    node_cost[neighbor_index] = parent_cost + temp_distance
                    goal_cost[neighbor_index] = math.sqrt(np.sum((neighbor_pt - goal_pt) ** 2))
                    node_pt[neighbor_index] = neighbor_pt
                    node_angle[neighbor_index] = theta_temp
                    track.update({neighbor_index: parent_index})
                    path.update({neighbor_index: {'x': temp_x, 'y': temp_y, 'UL': omega_left, 'UR': omega_right}})

                    if neighbor_index not in unvisited_nodes_index:
                        unvisited_nodes_index.append(neighbor_index)

                    iter_bin[-1].append({})  # updating an empty list for each action as an empty dictionary
                    iter_bin[-1][-1].update({'x': temp_x, 'y': temp_y})

    node_flag[parent_index] = 1
    unvisited_nodes_index.remove(parent_index)
    visited_nodes_index.append(parent_index)
    return 0

# -------------------------------------------------Parameters-----------------------------------------------------------
r_wheel = 0.038  # in m, radius of wheels
L_wheel = 0.354  # in m, distance between two wheels
# r_robot = 0.177  # in m, radius of robot
dt = 0.1   # in s, differential time needed for integration
threshold_distance = 0.1  # in m, threshold distance between each node
goal_threshold_radius = 0.2  # in m, threshold radius for goal node
# clearance_obstacle = 0.123  # in m, clearance for obstacles
clearance = 0.25  # clearance_obstacle + r_robot  # in m, effective clearance

# -----------------------------------------Discretizing the workspace---------------------------------------------------
X_ = np.linspace(0, 10, int(10 / threshold_distance) + 1)
Y_ = np.linspace(0, 10, int(10 / threshold_distance) + 1)
X = []
Y = []
for ix in range(len(X_)):
    for iy in range(len(Y_)):
        X.append(X_[ix])
        Y.append(Y_[iy])

# --------------------------------------------------Variables-----------------------------------------------------------
node_pt = np.zeros((len(X), 2))  # X & Y coordinates for each node
node_angle = np.zeros(len(X))  # orientation (in radians) of robot at each node
node_flag = np.zeros(len(X))  # will take either 0 (if unvisited) or 1 (if visited) for each node
node_cost = np.zeros(len(X)) + np.inf  # cost to go from start pt for each node
goal_cost = np.zeros(len(X)) + np.inf  # cost to go from each node to goal pt
visited_nodes_index = []  # keeps a list of visited nodes
track = {}  # tracks parent node from child node
path = {}  # stores coordinates and angular velocities of both the wheels for every action with child index as key
iter_bin = []  # stores coordinates for every action with iterator as key

X_start = 1.0
Y_start = 1.0
angle_start = 0.0  # in radian
start_pt = np.array([X_start, Y_start])
start_index = find_index(start_pt)

X_goal = 9.0
Y_goal = 9.0
goal_pt = np.array([X_goal, Y_goal])
goal_index = find_index(goal_pt)

# ----------------------------------------------------------------------------------------------------------------------
node_pt[start_index] = start_pt
node_cost[start_index] = 0.0  # assigning cost of start node to zero
goal_cost[start_index] = math.sqrt(np.sum((start_pt - goal_pt) ** 2))
node_angle[start_index] = angle_start
unvisited_nodes_index = [start_index]

# ----------------------------------------------------------------------------------------------------------------------
start_time = time.time()
iterator = 0
goal_flag = 0

print('\n\nSolving.........')
print('\nIteration # \t Time (mins.)\n')

while goal_flag == 0:

    if iterator % 100 == 0 and iterator != 0:
        mid_time = (time.time() - start_time) / 60
        print(' {0} \t\t {1:1.3f}'.format(iterator, mid_time))

    temp_cost = node_cost[unvisited_nodes_index] + goal_cost[unvisited_nodes_index]
    temp = np.argmin(temp_cost)
    next_node_index = unvisited_nodes_index[temp]

    if goal_cost[next_node_index] <= goal_threshold_radius:
        goal_flag = 1
        iterator += 1
        end_time = time.time()
        total_time = (end_time - start_time) / 60
        print('\n\nNumber of iterations taken to reach goal state: {}'.format(iterator))
        print('\nTime taken to find optimal (shortest) path: {0:1.3f} min'.format(total_time))

        node_flag[next_node_index] = 1
        unvisited_nodes_index.remove(next_node_index)
        visited_nodes_index.append(next_node_index)
        print('\n\nRobot reached within the threshold of goal node ...!')
        print('\nCurrent node number for robot:', next_node_index)
        print('Location (x, y):', node_pt[next_node_index])
        print('Cost:', node_cost[next_node_index])
        break

    goal_flag = a_star(next_node_index)
    iterator += 1

# -----------------------------------------------Visited Node Exploration-----------------------------------------------
x_explore = []
y_explore = []

for i in range(len(iter_bin)):  # for each iteration
    for j in range(len(iter_bin[i])):  # for each action
        x_explore.append(iter_bin[i][j]['x'])
        y_explore.append(iter_bin[i][j]['y'])

# -----------------------------------------------Optimal solution trajectory--------------------------------------------
back_track = []
x_solution = []
y_solution = []

def traj(child):
    if child != start_index:
        back_track.append(child)
        parent = track[child]
        return traj(parent)

    else:
        back_track.append(start_index)
        return back_track[::-1]

trajectory = traj(visited_nodes_index[-1])

for i in range(1, len(trajectory)):
    child_node = trajectory[i]
    x_solution.append(path[child_node]['x'])
    y_solution.append(path[child_node]['y'])

# --------------------------------------- Visualization starts from here -----------------------------------------------
print('\n\n### Creating Visualization ###')
start_time_plot = time.time()

plt.style.use('seaborn-pastel')
fig = plt.figure()
ax = plt.axes(xlim=(0, 10), ylim=(0, 10))  # Defining Workspace limits
ax.set_aspect('equal')

# For Plotting Circle 1 with center at (2, 2) and radius 1 unit
x_circle1 = np.linspace(1, 3, 2000)
y_circle1a = 2 + (1 ** 2 - (x_circle1 - 2) ** 2) ** 0.5
y_circle1b = 2 - (1 ** 2 - (x_circle1 - 2) ** 2) ** 0.5
ax.plot(x_circle1, y_circle1a, 'b.', markersize=0.15)
ax.plot(x_circle1, y_circle1b, 'b.', markersize=0.15)

# For Plotting Circle 2 with center at (2, 8) and radius 1 unit
x_circle2 = np.linspace(1, 3, 2000)
y_circle2a = 8 + (1 ** 2 - (x_circle1 - 2) ** 2) ** 0.5
y_circle2b = 8 - (1 ** 2 - (x_circle1 - 2) ** 2) ** 0.5
ax.plot(x_circle2, y_circle2a, 'b.', markersize=0.15)
ax.plot(x_circle2, y_circle2b, 'b.', markersize=0.15)

# For Plotting Square with center at (1, 5) and dimension 1.5x1.5
x1, y1 = (0.25, 4.25)
x2, y2 = (0.25, 5.75)
x3, y3 = (1.75, 5.75)
x4, y4 = (1.75, 4.25)
ax.plot([x1, x2], [y1, y2], 'b-')
ax.plot([x2, x3], [y2, y3], 'b-')
ax.plot([x3, x4], [y3, y4], 'b-')
ax.plot([x4, x1], [y4, y1], 'b-')

# For Plotting Rectangle 1 with center at (5, 5) and dimension 2.5x1.5
x5, y5 = (3.75, 4.25)
x6, y6 = (3.75, 5.75)
x7, y7 = (6.25, 5.75)
x8, y8 = (6.25, 4.25)
ax.plot([x5, x6], [y5, y6], 'b-')
ax.plot([x6, x7], [y6, y7], 'b-')
ax.plot([x7, x8], [y7, y8], 'b-')
ax.plot([x8, x5], [y8, y5], 'b-')

# For Plotting Rectangle 2 with center at (8, 3) and dimension 1.5x2.0
x9, y9 = (7.25, 2.0)
x10, y10 = (7.25, 4.0)
x11, y11 = (8.75, 4.0)
x12, y12 = (8.75, 2.0)
ax.plot([x9, x10], [y9, y10], 'b-')
ax.plot([x10, x11], [y10, y11], 'b-')
ax.plot([x11, x12], [y11, y12], 'b-')
ax.plot([x12, x9], [y12, y9], 'b-')

# For Plotting Circle threshold for goal node
x_goal_circle = np.linspace(X_goal - goal_threshold_radius, X_goal + goal_threshold_radius, 2000)
y_goal_circle1 = Y_goal + (goal_threshold_radius ** 2 - (x_goal_circle - X_goal) ** 2) ** 0.5
y_goal_circle2 = Y_goal - (goal_threshold_radius ** 2 - (x_goal_circle - X_goal) ** 2) ** 0.5
ax.plot(x_goal_circle, y_goal_circle1, 'y.', markersize=20)
ax.plot(x_goal_circle, y_goal_circle2, 'y.', markersize=20)

new_node, = ax.plot([], [], 'g.')
solution_trajectory, = ax.plot([], [], 'r.')

def animate(frame_number):
    """
        In this function, animation is carried out.

        Parameters
        ----------
        frame_number : int type, here frame number serves as an index for the images

        Returns
        -------
        None
    """

    frame_diff = total_frames - frame_number

    if frame_diff > 51:  # will run for frame_number = [0, 148]
        first = 0
        last = step1 * (frame_number + 1)
        x = x_explore[first:last]
        y = y_explore[first:last]
        new_node.set_data(x, y)
        new_node.set_markersize(1)
        return new_node,

    elif frame_diff == 51:  # will run for frame_number = 149 only
        x = x_explore
        y = y_explore
        new_node.set_data(x, y)
        new_node.set_markersize(1)
        return new_node,

    elif 51 > frame_diff > 1:  # will run for frame_number = [150, 198]
        first = 0
        last = step2 * (frame_number - 149)
        x = x_solution[first:last]
        y = y_solution[first:last]
        solution_trajectory.set_data(x, y)
        solution_trajectory.set_markersize(1.5)
        return solution_trajectory,

    else:  # will run for frame_number = 199 only
        x = x_solution
        y = y_solution
        solution_trajectory.set_data(x, y)
        solution_trajectory.set_markersize(1.5)
        return solution_trajectory,

node_explore_frames = 150
solution_traj_frames = 50
total_frames = node_explore_frames + solution_traj_frames

step1 = int(len(x_explore) / node_explore_frames)
step2 = int(len(x_solution) / solution_traj_frames)

# animation = FuncAnimation(fig, animate, frames=total_frames, interval=30, blit=True, repeat=False)

# animation.save('Differential Drive Visualization (A-star) for test case (1 0.5 0).mp4', dpi=300)

# plt.close()

ax.plot(x_explore, y_explore, 'g.', markersize=1.0)
ax.plot(x_solution, y_solution, 'r.', markersize=1.0)
plt.show()

end_time_plot = time.time()
total_time_plot = (end_time_plot - start_time_plot)
print('\n\nTime taken for making visualization: {0:1.3f} s'.format(total_time_plot))

# --------------------------------------- Writing coordinates and velocities for Gazebo---------------------------------
print('\n\n\nWriting coordinates and velocities for Gazebo')
n_action = len(trajectory)
control_data = np.zeros((n_action, 4))

for control in range(0, n_action - 1):
    control_index = trajectory[control]
    x_old, y_old = node_pt[control_index]
    theta_old = node_angle[control_index]
    UL = path[trajectory[control + 1]]['UL']
    UR = path[trajectory[control + 1]]['UR']

    #shifting origin to Gazebo world coordinates
    x_new, y_new = (x_old - 5, y_old - 5)
    v_x1 = (0.5 * r_wheel) * (UL + UR) * math.cos(theta_old)
    v_x2 = (0.5 * r_wheel) * (UL + UR) * math.sin(theta_old)
    v_x = math.sqrt(v_x1**2 + v_x2**2)
    w_z = (r_wheel / L_wheel) * (UR - UL)

    control_data[control] = np.array([x_new, y_new, v_x, w_z])

control_data[-1][:2] = np.array(node_pt[trajectory[-1]])

np.savetxt('a_star_controls.txt', control_data)

print('\nCompleted !!!')