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

import sys
import os.path
from os import path
import csv
import random

"""Module for generating random poses of an three-dimensional object. 
   Usage:
   > python RandomPoses.py <limitsFile> <numberOfSamples>
   
   Input arguments:
   * limitsFile - a path to csv file containing two rows and six columns. 
                  The first row describes minimum limits, the second row maximum
                  limits of the deviation from the reference pose. The colums
                  contain deviation for x,y,z rotation and x,y,z translation 
                  respectively.
   * numberOfSamples - number of random poses to be generated.
   
   The random deviations to the reference pose are printed to the standard
   output.
"""

__author__ = "Ondrej Klima"
__copyright__ = "Copyright 2020"
__credits__ = ["Ondrej Klima"]
__email__ = "iklima@fit.vutbr.cz"
__license__ = "BUT"
__version__ = "1.0"
__maintainer__ = "Ondrej Klima"

def main():
    # Parsing input arguments 
    argv = sys.argv
    
    try:
        limitsFileName = argv[1]
    except IndexError:
        raise IndexError('File with deviation limits must be supplied as an argument')
    if not path.isfile(limitsFileName):
        raise ValueError('File "%s" does not exist!' % limitsFileName)
    
    try:
        N = int(argv[2])
    except IndexError:
        raise IndexError('Number of samples to be generated must be supplied as an argument')

    (minRX, minRY, minRZ, minTX, minTY, minTZ) = (.0, .0, .0, .0, .0, .0)
    (maxRX, maxRY, maxRZ, maxTX, maxTY, maxTZ) = (.0, .0, .0, .0, .0, .0)   
    with open(limitsFileName, newline='') as csvfile:
        reader = csv.reader(csvfile, delimiter=',', quotechar='|')
        data = list(reader)
        
        rowi = 0
        for row in data:
            if rowi == 0:
                (minRX, minRY, minRZ) = (float(row[0]), float(row[1]), float(row[2]))
                (minTX, minTY, minTZ) = (float(row[3]), float(row[4]), float(row[5]))    
            elif rowi == 1:
                (maxRX, maxRY, maxRZ) = (float(row[0]), float(row[1]), float(row[2]))
                (maxTX, maxTY, maxTZ) = (float(row[3]), float(row[4]), float(row[5]))  
            rowi = rowi + 1             

    for i in range(N):

        # Rotation is in degree units
        print(random.uniform(minRX, maxRX), end = ',')
        print(random.uniform(minRY, maxRY), end = ',')
        print(random.uniform(minRZ, maxRZ), end = ',')

        # Translation is in meter units
        print(random.uniform(minTX, maxTX) / 1000.0, end = ',')
        print(random.uniform(minTY, maxTY) / 1000.0, end = ',')
        print(random.uniform(minTZ, maxTZ) / 1000.0)


if __name__ == "__main__":
    main()