#!/usr/bin/env python

from dynamic_reconfigure.parameter_generator_catkin import *

PACKAGE="asr_robot_model_services"

gen = ParameterGenerator()
camera = gen.add_group("Camera")
robotModel = gen.add_group("Robot Model")
rating = gen.add_group("Rating")
vis = gen.add_group("Visualization")
moduleChooser = gen.add_group("Module Chooser")
other = gen.add_group("Other")

# Name, Type, Reconfiguration level, 
# Description, 
# Default, Min, Max
  
# mapHelper
robotModel.add("colThresh", double_t, (0b1 << 2), 
  "", 
  45.0, 0.1, 100.0)

# camera
# ncp and fcp are also used by robot model
camera.add("ncp", double_t, (0b1 << 4) | (0b1 << 5), 
  "Distance to NearClippingPlane in meter", 
  .5, 0.0, 2.0)
camera.add("fcp", double_t, (0b1 << 4) | (0b1 << 5), 
  "Distance to FarClippingPlane in meter", 
  5.0, 1.0, 10.0)
camera.add("fovy", double_t, 0b1 << 4, 
  "angle of vertical field of view in degree", 
  48.9, 10, 180)

# robotmodel
# some of these might be not be applied immediately, because they are read from rosparam instead of config struct.
robotModel.add("useGlobalPlanner", bool_t, 0b1 << 5, 
  "", 
  True)
robotModel.add("tolerance", double_t, 0b1 << 5, 
  "", 
  0.00003, 0.0, 10.0)
robotModel.add("panMin", double_t, 0b1 << 5, 
  "", 
  -60, -360, 0)
robotModel.add("panMax", double_t, 0b1 << 5, 
  "", 
  60, 0, 360)
robotModel.add("tiltMin", double_t, 0b1 << 5, 
  "", 
  -45, -360, 0)
robotModel.add("tiltMax", double_t, 0b1 << 5, 
  "", 
  45, 0, 360)
robotModel.add("mSigma", double_t, 0b1 << 5, 
  "", 
  1.0)
robotModel.add("panAngleSamplingStepsPerIteration", int_t, 0b1 << 5, 
  "", 
  20, 1, 1000000000)
robotModel.add("inverseKinematicIterationAccuracy", double_t, 0b1 << 5, 
  "", 
  0.005, 0.0, 10.0)

# rating
# rot/tilt/pan are used by MILDRobotModel
rating.add("mOmegaPan", double_t, (0b1 << 6) | (0b1 << 5), 
  "coefficient for the rotation of the pan axis of the PTU", 
  1, 0.0, 1000.0)
rating.add("mOmegaTilt", double_t, (0b1 << 6) | (0b1 << 5), 
  "coefficient for the rotation of the tilt axis of the PTU", 
  1, 0.0, 1000.0)
rating.add("mOmegaRot", double_t, (0b1 << 6) | (0b1 << 5), 
  "coefficient for the rotation of the base", 
  1, 0.0, 1000.0)
rating.add("mOmegaBase", double_t, 0b1 << 6, 
  "coefficient for the movement of the base", 
  1, 0.0, 1000.0)

# vis
vis.add("visualizeIK", bool_t, 0b1 << 0, 
  "", 
  False)

# module chooser
robotModelEnum = gen.enum([ 
    gen.const("MILDRobotModelWithExactIK",      	int_t, 1, ""),
    gen.const("MILDRobotModelWithApproximatedIK", 	int_t, 2, "")
  ],
  "An enum to set the robot model")
moduleChooser.add("robotModelId", int_t, 0b1 << 5, "robot model", 1, 1, 2, edit_method=robotModelEnum)

# other
# mCropboxListFilePath is level 8, so cropbox xml is only reloaded if a cropboxsetting is changed
other.add("enableCropBoxFiltering", bool_t, 0b1 << 0, 
  "", 
  False)
other.add("mCropBoxListFilePath", str_t, 0b1 << 8, 
  "", 
  "NONE")
other.add("epsilon", double_t, 0b1 << 0,
  "",
  0.001, 10e-10, 1)
other.add("debugLevels", str_t, 0b1 << 0, 
  "", 
  "NONE")

exit(gen.generate(PACKAGE, "robot_model_services", "DynamicParameters"))