#!/usr/bin/python
# Software License Agreement (BSD License)
#
# Copyright (C) 2012, Austin Robot Technology
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Austin Robot Technology, Inc. nor the names
#    of its contributors may be used to endorse or promote products
#    derived from this software without specific prior written
#    permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id$

"""
Generate YAML calibration file from Velodyne db.xml.

The input data provided by the manufacturer are in degrees and
centimeters. The YAML file uses radians and meters, following ROS
standards [REP-0103].

"""

from __future__ import print_function

import math
import optparse
import os
import sys
from xml.etree import ElementTree
import yaml

# parse the command line
usage = """usage: %prog infile.xml [outfile.yaml]

       Default output file is input file with .yaml suffix."""
parser = optparse.OptionParser(usage=usage)
options, args = parser.parse_args()

if len(args) < 1:
    parser.error('XML file name missing')
    sys.exit(9)

xmlFile = args[0]
if len(args) >= 2:
    yamlFile = args[1]
else:
    yamlFile, ext = os.path.splitext(xmlFile)
    yamlFile += '.yaml'

print('converting "' + xmlFile + '" to "' + yamlFile + '"')

calibrationGood = True
def xmlError(msg):
    'handle XML calibration error'
    global calibrationGood
    calibrationGood = False
    print('gen_calibration.py: ' + msg)

db = None
try:
    db = ElementTree.parse(xmlFile)
except IOError:
    xmlError('unable to read ' + xmlFile)
except ElementTree.ParseError:
    xmlError('XML parse failed for ' + xmlFile)

if not calibrationGood:
    sys.exit(2)

# create a dictionary to hold all relevant calibration values
calibration = {'num_lasers': 0, 'lasers': [], 'distance_resolution': 0.2}
cm2meters = 0.01                       # convert centimeters to meters

def addLaserCalibration(laser_num, key, val):
    'Define key and corresponding value for laser_num'
    global calibration
    if laser_num < len(calibration['lasers']):
        calibration['lasers'][laser_num][key] = val
    else:
        calibration['lasers'].append({key: val})

# add enabled flags
num_enabled = 0
enabled_lasers = []
enabled = db.find('DB/enabled_')
if enabled == None:
    print('no enabled tags found: assuming all 64 enabled')
    num_enabled = 64
    enabled_lasers = [True for i in xrange(num_enabled)]
else:
    index = 0
    for el in enabled:
        if el.tag == 'item':
            this_enabled = int(el.text) != 0
            enabled_lasers.append(this_enabled)
            index += 1
            if this_enabled:
                num_enabled += 1

calibration['num_lasers'] = num_enabled
print(str(num_enabled) + ' lasers')

# add distance resolution (cm)
distLSB = db.find('DB/distLSB_')
if distLSB != None:
    calibration['distance_resolution'] = float(distLSB.text) * cm2meters

# add minimum laser intensities
minIntensities = db.find('DB/minIntensity_')
if minIntensities != None:
    index = 0
    for el in minIntensities:
        if el.tag == 'item':
            if enabled_lasers[index]:
                value = int(el.text)
                if value != 0:
                    addLaserCalibration(index, 'min_intensity', value)
            index += 1

# add maximum laser intensities
maxIntensities = db.find('DB/maxIntensity_')
if maxIntensities != None:
    index = 0
    for el in maxIntensities:
        if el.tag == 'item':
            if enabled_lasers[index]:
                value = int(el.text)
                if value != 255:
                    addLaserCalibration(index, 'max_intensity', value)
                index += 1

# add calibration information for each laser
for el in db.find('DB/points_'):
    if el.tag == 'item':
        for px in el:
            for field in px:
                if field.tag == 'id_':
                    index = int(field.text)
                    if not enabled_lasers[index]:
                        break   # skip this laser, it is not enabled
                    addLaserCalibration(index, 'laser_id', index)

                if field.tag == 'rotCorrection_':
                    addLaserCalibration(index, 'rot_correction',
                                        math.radians(float(field.text)))
                elif field.tag == 'vertCorrection_':
                    addLaserCalibration(index, 'vert_correction',
                                        math.radians(float(field.text)))
                elif field.tag == 'distCorrection_':
                    addLaserCalibration(index, 'dist_correction', 
                                        float(field.text) * cm2meters)
                elif field.tag == 'distCorrectionX_':
                    addLaserCalibration(index, 'dist_correction_x',
                                        float(field.text) * cm2meters)
                elif field.tag == 'distCorrectionY_':
                    addLaserCalibration(index, 'dist_correction_y',
                                        float(field.text) * cm2meters)
                elif field.tag == 'vertOffsetCorrection_':
                    addLaserCalibration(index, 'vert_offset_correction',
                                        float(field.text) * cm2meters)
                elif field.tag == 'horizOffsetCorrection_':
                    addLaserCalibration(index, 'horiz_offset_correction',
                                        float(field.text) * cm2meters)
                elif field.tag == 'focalDistance_':
                    addLaserCalibration(index, 'focal_distance', 
                                        float(field.text) * cm2meters)
                elif field.tag == 'focalSlope_':
                    addLaserCalibration(index, 'focal_slope', float(field.text))

# validate input data
if calibration['num_lasers'] <= 0:
    xmlError('no lasers defined')
elif calibration['num_lasers'] != num_enabled:
    xmlError('inconsistent number of lasers defined')

# TODO: make sure all required fields are present.
# (Which ones are required?)

if calibrationGood:

    # write calibration data to YAML file
    f = open(yamlFile, 'w')
    try:
        yaml.dump(calibration, f)
    finally:
        f.close()