#!/usr/bin/env python

import rospy
from std_msgs.msg import Float32, UInt16, String


import lowlevel_control
from lowlevel_control.pid_data import PID_Data
import lowlevel_control.pid_control as _PID

data = PID_Data()


controller = _PID.PID(1.0, 2.0, 3.0, 1.0, -1.0)


def node_publisher():
    """
    Publish rudder servo angle data (UInt16) to Arduino node.
    Higher level tack angle was used when in TACK manoeuvre.
    PID controller was used in other conditions.
    :rtype: object
    """
    pub = rospy.Publisher('/servo', UInt16, queue_size=10)
    rospy.init_node('rudder_servo', anonymous=True)
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        if data.update_sailing_state != 'TACK':
            angle = controller.update_PID(data.goal_heading-data.heading)
        else:
            angle = data.tack_rudder

        pub.publish(angle)

        rate.sleep()

print(data.goal_heading)

if __name__ == '__main__':
    try:
        rospy.Subscriber('/goal_heading', Float32, data.update_goal_heading)
        rospy.Subscriber('/heading', Float32, data.update_heading)
        rospy.Subscriber('/sailing_state', String, data.update_sailing_state)
        rospy.Subscriber('/tack_rudder', Float32, data.update_tack_rudder)
        node_publisher()
    except rospy.ROSInterruptException:
        pass
