#!/usr/bin/env python

import rospy

import cv2
import numpy as np
import math

from sensor_msgs.msg import LaserScan
from mavros_msgs.msg import OverrideRCIn


def callback(data):
    pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
    frame = np.zeros((500, 500, 3), np.uint8)

    pitch = 1500
    yaw = 1500
    max_yaw = 1601
    max_pitch = 1445

    min_yaw = 1345
    min_pitch = 1455

    angle = data.angle_max
    Vx = 250
    Vy = 250
    for r in data.ranges:
        if r == float('Inf'):
            r = data.range_max
        x = math.trunc((r * 10) * math.cos(angle + (-90 * 3.1416 / 180)))
        y = math.trunc((r * 10) * math.sin(angle + (-90 * 3.1416 / 180)))
        cv2.line(frame, (250, 250), (x + 250, y + 250), (255, 0, 0), 1)
        Vx += x
        Vy += y
        angle = angle - data.angle_increment

    cv2.line(frame, (250, 250), (250 + Vx, 250 + Vy), (0, 0, 255), 3)
    cv2.circle(frame, (250, 250), 2, (255, 255, 0))
    ang = -(math.atan2(Vx, Vy) - 3.1416) * 180 / 3.1416
    if ang > 180:
        ang -= 360
    cv2.putText(frame, str(ang)[:10], (50, 400), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 255, 255))

    cv2.imshow('frame', frame)
    cv2.waitKey(1)

    # calculate YAW with data from OpenCV
    if ang > 0:
        yaw = 1500 + ang * 40 / 6 + 80
        print ""
        print "POSITIVE ang " + str(ang)
        print "  yaw " + str(ang * 40 / 6)
        print "  pos " + str(yaw)
        print ""
        if yaw > max_yaw:
            yaw = max_yaw
    else:
        yaw = 1500 + ang * 40 / 6 - 110
        print ""
        print "NEGATIVE ang " + str(ang)
        print "  yaw " + str(ang * 40 / 6)
        print "  pos " + str(yaw)
        print ""
        if yaw < min_yaw:
            yaw = min_yaw

    # stabelizen
    if round(yaw) == 1600:
        yaw = 1500

    if 1580 > round(yaw) > 1450:
        pitch = max_pitch

    if round(yaw) == max_yaw or round(yaw) == min_yaw:
        pitch = 1500

    throttle = 1500

    msg = OverrideRCIn()
    msg.channels[0] = 1500  # x > 1500 = rechts, x < 1500 = links
    msg.channels[1] = pitch  # x > 1500 = achter, x < 1500 = naar voren (MODE LOITER NODIG)
    msg.channels[2] = throttle  # x > 1500 = stijgen, x < 1500 = dalen
    msg.channels[3] = round(yaw)
    msg.channels[4] = 1000
    msg.channels[5] = 1000
    msg.channels[6] = 1000
    msg.channels[7] = 1000
    print msg
    pub.publish(msg)


def laser_listener():
    rospy.init_node('laser_listener', anonymous=True)

    rospy.Subscriber("/scan", LaserScan, callback)
    rospy.spin()

if __name__ == '__main__':
    laser_listener()