#!/usr/bin/env python

import rospy
import tf
import math 

from geometry_msgs.msg          import PoseStamped, Point, PoseWithCovariance, TwistWithCovariance, Quaternion
from nav_msgs.msg               import Odometry, Path
from visualization_msgs.msg     import Marker
from std_msgs.msg               import Header
from vn300.msg                  import gps, ins

class OdomToPath:
    def __init__(self):
        self.odom_sub = rospy.Subscriber('/vectornav/ins',       ins,  self.to_path, queue_size=10)
        self.odom_pub = rospy.Publisher( '/golfcart_ekf/truth_odom',  Odometry,   latch=True, queue_size=10)
        
        self.br = tf.TransformBroadcaster()
        self.odom = Odometry()
        self.odom.header.frame_id = 'world'
        self.odom.child_frame_id = 'world' # NED Velocities

        self.time_init = False
        self.time_prev = rospy.rostime.Time()
        self.header_prev = Header()
        self.c_yaw = 0.0


    def to_path(self, msg):
        if not self.time_init:
            self.time_init = True
            self.lat_home = msg.LLA.x
            self.lon_home = msg.LLA.y
            rospy.loginfo("[ins_to_odom] Initial LLA X %f, LLA Y %f",self.lat_home,self.lon_home)

        else:
            # Ellipsoidal model calculation of Earth
            a = 6378137.0000
            b = 6356752.3142
            R = math.sqrt(( math.pow( math.pow(a,2) * math.cos(msg.LLA.x*math.pi/180), 2)+ \
                            math.pow( math.pow(b,2) * math.sin(msg.LLA.x*math.pi/180), 2))/ \
                           (math.pow( a * math.cos(msg.LLA.x*math.pi/180), 2)+ \
                            math.pow( b * math.sin(msg.LLA.x*math.pi/180), 2))) + msg.LLA.z

            # Transform LLH to 2D position
            dlon = msg.LLA.y - self.lon_home
            dlat = msg.LLA.x - self.lat_home
            dE = R * math.radians(dlon)     
            dN = R * math.radians(dlat)
            c_yaw =  (90 - msg.RPY.z )* (math.pi/180.0)
            self.c_yaw = c_yaw # quick fix     

            point = Point(dE,dN,0)

            pose = PoseWithCovariance()
            pose.pose.position = point
            pose.pose.orientation = Quaternion(0,0, math.sin(c_yaw/2),math.cos(c_yaw/2))
            pose.covariance[0] = msg.PosUncertainty**2
            pose.covariance[1] = msg.PosUncertainty**2  
            pose.covariance[35] = msg.YawUncertainty**2
            
            twist = TwistWithCovariance()
            # twist.twist.linear.x = msg.NedVel.y*math.cos(c_yaw) + msg.NedVel.x*math.sin(c_yaw)
            # twist.twist.linear.y = -msg.NedVel.y*math.sin(c_yaw) + msg.NedVel.x*math.cos(c_yaw)
            twist.twist.linear.x = msg.NedVel.x
            twist.twist.linear.y = msg.NedVel.y

            self.odom.header.frame_id = 'world'
            self.odom.header.stamp = rospy.Time.now()
            # self.odom.header.seq = msg.header.seq
            self.odom.pose = pose
            self.odom.twist = twist

            self.odom_pub.publish(self.odom)
        self.br.sendTransform(  (self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, 0.0),
                        tf.transformations.quaternion_from_euler(0, 0, self.c_yaw),
                        msg.header.stamp,
                        'vectornav',
                        'world')
        self.time_prev = msg.header.stamp.to_sec()



    def run(self):
        
        rate = rospy.Rate(50.0) 
        while(not rospy.is_shutdown()):
            #self.c_yaw = 0.0 # quick fix

            rate.sleep()


if __name__ == '__main__':
    rospy.init_node('ins_to_path')
    odom_to_path = OdomToPath()
    odom_to_path.run()