#!/usr/bin/env python3
"""
angles.py : the two angle functions from Lecture 2, which you will write again in Lab 1.

    python3 angles.py            # the round trip and the wrap trap from the slides

yaw_from_quaternion reads the heading out of a ROS quaternion (x, y, z, w).
wrap puts any angle in (-pi, pi], the ROS convention; wrap every angle difference before using it as an error.
"""
import math
from collections import namedtuple

Quaternion = namedtuple('Quaternion', 'x y z w')


def yaw_from_quaternion(q):
    siny = 2.0 * (q.w * q.z + q.x * q.y)
    cosy = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
    return math.atan2(siny, cosy)


def quaternion_from_yaw(yaw):
    """A rotation of yaw radians about z: (0, 0, sin(yaw/2), cos(yaw/2))."""
    return Quaternion(0.0, 0.0, math.sin(yaw / 2), math.cos(yaw / 2))


def wrap(a):
    return math.atan2(math.sin(a), math.cos(a))


def main():
    yaw = math.radians(30.0); q = quaternion_from_yaw(yaw); neg = Quaternion(-q.x, -q.y, -q.z, -q.w)
    print('yaw   = %.6f deg' % math.degrees(yaw))
    print('q     = (%g, %g, %.6f, %.6f)' % q)
    print('back  = %.6f deg' % math.degrees(yaw_from_quaternion(q)))
    print('-q    = %.6f deg  (same)' % math.degrees(yaw_from_quaternion(neg)))
    goal, current, rate = math.radians(175.0), math.radians(-175.0), 1.90
    naive, wrapped = goal - current, wrap(goal - current)
    print('naive:   %+.0f deg, %.2f s at %.2f rad/s' % (math.degrees(naive), abs(naive) / rate, rate))
    print('wrapped: %+.0f deg, %.2f s, %.0fx shorter' % (math.degrees(wrapped), abs(wrapped) / rate, abs(naive) / abs(wrapped)))


if __name__ == '__main__':
    main()