#!/usr/bin/env python

import os
import numpy
import threading
import math

import roslib
roslib.load_manifest('hydrophones')
import rospy

from hydrophones import algorithms, util
from hydrophones.msg import Ping, ProcessedPing, Debug
from std_msgs.msg import Header
from geometry_msgs.msg import Point, PoseStamped, Pose
import tf
from tf import transformations

def process_ping(ping):
    samples = util.ping_to_samples(ping)
    sample_rate = ping.sample_rate

    r = algorithms.run(samples, sample_rate, v_sound, dist_h, dist_h4)

    valid = False
    heading = 0
    declination = 0
    try:
        _, quat = tf_listener.lookupTransform(output_frame, ping.header.frame_id, ping.header.stamp)
        if len(r['pos']) > 0:
            pos_output = transformations.quaternion_matrix(quat)[:3, :3].dot(r['pos'])
            heading = math.atan2(pos_output[1], pos_output[0])
            declination = math.atan2(-pos_output[2], numpy.linalg.norm(pos_output[0:2]))
            if len(r['errors']) == 0:
                valid = True
    except tf.Exception, ex:
        rospy.logerr('Cannot output processed ping because of tf exception: ' + str(ex))

    if len(r['errors']) > 0:
        rospy.logwarn('Errors processing ping: ' + ", ".join(r['errors']))

    pub.publish(header=Header(stamp=ping.header.stamp,
                              frame_id=output_frame),
                heading=heading,
                declination=declination,
                freq=r['freq'],
                amplitude=r['amplitude'],
                valid=valid)
    pub_pose.publish(header=Header(stamp=ping.header.stamp,
                                   frame_id=ping.header.frame_id),
    pose=Pose(position=Point(*r['pos'].tolist())))
    pub_debug.publish(header=Header(stamp=ping.header.stamp,
                                    frame_id=ping.header.frame_id),
                      deltas=r['deltas'].tolist(),
                      delta_errors=r['delta_errors'].tolist(),
                      fft_sharpness=r['fft_sharpness'])

rospy.init_node('hydrophones')
dist_h = rospy.get_param('~dist_h')
dist_h4 = rospy.get_param('~dist_h4')
v_sound = rospy.get_param('~v_sound')
template_periods = rospy.get_param('~template_periods', 3)
output_frame = rospy.get_param('~output_frame', '/base_link')
tf_listener = tf.TransformListener()
pub = rospy.Publisher('hydrophones/processed', ProcessedPing)
pub_pose = rospy.Publisher('hydrophones/pose', PoseStamped)
pub_debug = rospy.Publisher('hydrophones/debug', Debug)
sub = rospy.Subscriber('hydrophones/ping', Ping, process_ping)
rospy.spin()
