#!/usr/bin/env python

PACKAGE = "sensorfusion"
from dynamic_reconfigure.parameter_generator_catkin import *
import roslib;
roslib.load_manifest(PACKAGE)

gen = ParameterGenerator()
# Options for the sensorfusion module and its sub-modules
time_update_group = gen.add_group("Time update settings", type="collapse", state=True)
time_update_group.add("enable_time_update",bool_t, 0, "", True)

time_update_group.add("process_cov_quat_0", double_t, 0, "Process noise for q0", 0.001, 0.0000001, 10000)
time_update_group.add("process_cov_quat_1", double_t, 0, "Process noise for q1", 0.001, 0.0000001, 10000)
time_update_group.add("process_cov_quat_2", double_t, 0, "Process noise for q2", 0.001, 0.0000001, 10000)
time_update_group.add("process_cov_quat_3", double_t, 0, "Process noise for q3", 0.001, 0.0000001, 10000)
time_update_group.add("process_cov_p", double_t, 0, "Process noise for p", 0.01, 0.0000001, 10000)
time_update_group.add("process_cov_q", double_t, 0, "Process noise for q", 0.01, 0.0000001, 10000)
time_update_group.add("process_cov_r", double_t, 0, "Process noise for r", 0.01, 0.0000001, 10000)
time_update_group.add("process_cov_bias_p", double_t, 0, "Process noise for p bias", 0.0000001, 0.000000001, 10000)
time_update_group.add("process_cov_bias_q", double_t, 0, "Process noise for q bias", 0.0000001, 0.000000001, 10000)
time_update_group.add("process_cov_bias_r", double_t, 0, "Process noise for r bias", 0.0000001, 0.000000001, 10000)
time_update_group.add("process_cov_d", double_t, 0, "Process noise for depth", 1.0, 0.00000001, 10000)


initial_group = gen.add_group("Initial covariance", type="collapse", state=True)
initial_group.add("initial_cov_quat_0", double_t, 0, "Initial covraiance for quaternion 0 ", 1000, 0.1, 100000)
initial_group.add("initial_cov_quat_1", double_t, 0, "Initial covraiance for quaternion 1", 1000, 0.1, 100000)
initial_group.add("initial_cov_quat_2", double_t, 0, "Initial covraiance for quaternion 2", 1000, 0.1, 100000)
initial_group.add("initial_cov_quat_3", double_t, 0, "Initial covraiance for squaternion 3", 1000, 0.1, 100000)
initial_group.add("initial_cov_p", double_t, 0, "Initial covraiance for state p", 1000, 0.1, 100000)
initial_group.add("initial_cov_q", double_t, 0, "Initial covraiance for state q", 1000, 0.1, 100000)
initial_group.add("initial_cov_r", double_t, 0, "Initial covraiance for state r", 1000, 0.1, 100000)
initial_group.add("initial_cov_bias_p", double_t, 0, "Initial covraiance for p bias", 0.000000001, 0.000000001, 100000)
initial_group.add("initial_cov_bias_q", double_t, 0, "Initial covraiance for q bias", 0.000000001, 0.000000001, 100000)
initial_group.add("initial_cov_bias_r", double_t, 0, "Initial covraiance for r bias", 0.000000001, 0.000000001, 100000)
initial_group.add("initial_cov_d", double_t, 0, "Initial covraiance for depth", 1000, 0.0000001, 100000)

param_group = gen.add_group("Physical parameters", type="collapse", state=True)
param_group.add("g", double_t, 0, "Gravitational constant",9.82,8,11)
param_group.add("x_offset", double_t, 0, "offset of rear pressure sensor in the x direction [meters]", -0.15, -0.3, 0)
param_group.add("rho", double_t, 0,"Density of water in kg/m3",1000,900,1100)

acc_group = gen.add_group("Acc update settings",type="collapse", state=True)
acc_group.add("enable_acceleration_update", bool_t, 0,"",True)
acc_group.add("eps_acc", double_t, 0, "maximum difference between measured acceleration and g [m/s^2]",2, 0, 10)
acc_group.add("cov_acc_x", double_t, 0, "covariance for acceleration measurement in x",1.0, 0.0000001, 10000)
acc_group.add("cov_acc_y", double_t, 0, "covariance for acceleration measurement in y",1.0, 0.0000001, 10000)
acc_group.add("cov_acc_z", double_t, 0, "covariance for acceleration measurement in z",1.0, 0.0000001, 10000)

gyro_group = gen.add_group("Gyro update settings",type="collapse", state=True)
gyro_group.add("enable_gyro_update", bool_t, 0,"",True)
gyro_group.add("cov_gyro_x", double_t, 0, "covariance for gyro measurement around x",0.1, 0.0000001, 10000)
gyro_group.add("cov_gyro_y", double_t, 0, "covariance for gyro measurement around y",0.1, 0.0000001, 10000)
gyro_group.add("cov_gyro_z", double_t, 0, "covariance for gyro measurement around z",0.1, 0.0000001, 10000)

mag_group = gen.add_group("Mag update settings",type="collapse", state=True)
mag_group.add("enable_magnetometer_update", bool_t, 0,"",True)
mag_group.add("eps_mag", double_t, 0, "maximum difference between measured magnetic field and earths magnetic field [uTesla]",50,0,1000)
mag_group.add("cov_mag_x", double_t, 0, "covariance for magnetometer measurement in x",1000, 0.0000001, 10000)
mag_group.add("cov_mag_y", double_t, 0, "covariance for magnetometer measurement in y",1000, 0.0000001, 10000)
mag_group.add("cov_mag_z", double_t, 0, "covariance for magnetometer measurement in z",1000, 0.0000001, 10000)

pressure_group = gen.add_group("Pressure update settings",type="collapse", state=True)
pressure_group.add("enable_pressure_update", bool_t, 0,"",True)
pressure_group.add("cov_pressure", double_t, 0, "covariance for pressure measurement",1.0, 0.0000001, 10000)

# generate the config headers
exit(gen.generate("sensorfusion","ekf","ekf"))
