#!/usr/bin/env python

# parameters

import yaml
import argparse
import numpy as np
import collections

import constants as c


class MapConfig(object):

    def __init__(self, yamlConfFilename):
        with open(yamlConfFilename, 'r') as yamlConfFile:
            yamlConfig = yaml.load(yamlConfFile)
        objConfig = yamlConfig['objects']
        self.nObjects = objConfig['N']
        self.nFeatureDims = objConfig['featureVecLen']
        self.useOrientation = objConfig['haveOrientation']
        envConfig = yamlConfig['environment']
        self.envSize = np.array(envConfig['size'])
        self.envDims = envConfig['nDims']


def numpyToNativePython(numpy1dArray):
    listArray = list(numpy1dArray)
    for i in range(len(listArray)):
        listArray[i] = listArray[i].item()
    return listArray
            

def generateMap(yamlConfFilename, mapFilename):
    mapConfig = MapConfig(yamlConfFilename)

    objectList = []
    for _ in range(mapConfig.nObjects):
        thisObjectDict = {}
        npPosition, npOrientation = generatePose(mapConfig.envSize,
                                                 mapConfig.envDims)
        thisObjectDict[c.LM_POS_KEY] = numpyToNativePython(npPosition)
        features = np.random.random(mapConfig.nFeatureDims)
        thisObjectDict[c.LM_FEAT_KEY] = numpyToNativePython(features)
        
        if mapConfig.useOrientation:
            if type(npOrientation) is np.ndarray:
                orientation = numpyToNativePython(npOrientation)
            else:
                orientation = float(npOrientation)
            thisObjectDict[c.LM_ORNT_KEY] = orientation
        objectList.append(thisObjectDict)

    mapDict = {'size': numpyToNativePython(mapConfig.envSize),
               'landmarks': objectList}

    with open(mapFilename, 'w') as mapFile:
        yaml.dump(mapDict, mapFile)


def generatePose(envSize, envDims):
    position = envSize * np.random.random(envDims)
    # orientations are unit vectors pointing in a random direction
    if envDims == 2:
        orientation = 2 * np.pi * np.random.random()
    # TODO implement returning random quaternion for 3D case
    return position, orientation
    

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument('mapFilename', help='\
    The filename to which the yaml map will be written.')
    parser.add_argument('--config', default=c.MAP_CONFIG_FILE, help='\
    The name of the config file that defines the parameters of the map to be \
    generated.')
    args = parser.parse_args()
    generateMap(args.config, args.mapFilename)