#!/usr/bin/env python

# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020

@author: Maxi
"""

import re
import roslib
import rospy
import sys
import time

import std_msgs.msg as msg
from sensor_msgs.msg import JointState

import pigpio
import Pin

# Header header
    # uint seq
    # time stamp
    # string frame_id
# string name
# float position
# float velocity
# float effort

class Commander:
    def __init__(self):
        self.test = False
        """ constants """
        self.rotate_old = self.rotate = 0  # rad
        self.elevate_old = self.elevate = 0  # rad
        self.servo_old = self.servo = 0  # rad
        
        """ status """
        self.rotate_enable = False
        self.elevate_enable = True
        self.servo_enable = False

        """ setup ROS """
        rospy.init_node('Commander')
        self.pub = rospy.Publisher('command',
                                    msg.String,
                                    queue_size=10)
        self.pub_gs = rospy.Publisher('groundstation_monitoring',
                                    JointState,
                                    queue_size=10)

        self.listener_monitor = rospy.Subscriber('monitor',
                                                msg.String,
                                                self.callback)
        self.listener_interrupt = rospy.Subscriber('interrupt',
                                                msg.String,
                                                self.callback)
        self.ground_control = rospy.Subscriber('groundstation_command',
                                                JointState,
                                                self.callback_cmd)
                                                
        """ keep node running """
        while not rospy.is_shutdown():
            rospy.spin()

    def callback_cmd(self, cmd):
        self.old_cmd()
        rospy.loginfo(rospy.get_caller_id() + 'I heard the command \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
                        cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
        # self.pub_gs.publish(cmd)
        self.rotate = cmd.position[0]
        self.elevate = cmd.position[1]
        self.servo = cmd.position[2]
        self.convert_command()

    def old_cmd(self):
        self.rotate_old = self.rotate
        self.elevate_old = self.elevate
        self.servo_old = self.servo

    def convert_command(self):
        if not self.rotate == self.rotate_old and self.rotate_enable:
            self.rotate = int(2*3.1415*float(self.rotate))  # rad to deg
            self.pub.publish(msg.String('ROTATE ' + str(self.rotate)))
            
        if not self.elevate == self.elevate_old:
            if self.elevate < -1.4 and self.elevate_enable:
                self.pub.publish(msg.String('STOW'))
            elif self.elevate >= -1.4 and self.elevate <= 0.4 and self.elevate_enable:
                self.pub.publish(msg.String('ELEVATE'))
            elif self.elevate > 0.4:
                self.pub.publish(msg.String('TILT'))
                
        if not self.servo == self.servo_old and self.servo_enable:
            self.servo = int(2*3.1415*float(self.camera))  # rad to deg
            self.pub.publish(msg.String('SERVO ' + str(self.servo)))                       

    def callback(self, data):
        rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
                    data.data)
        if 'ELEVATE' in data.data:
            if 'ENABLE' in data.data:
                self.elevate_enable = True
            else:
                self.elevate_enable = False

        if 'ROTATE' in data.data:
            if 'ENABLE' in data.data:
                self.rotate_enable = True
            else:
                self.rotate_enable = False

        if 'SERVO' in data.data:
            if 'ENABLE' in data.data:
                self.servo_enable = True
            else:
                self.servo_enable = False

if __name__ == '__main__':
    try:
        Commander()
    except rospy.ROSInterruptException:
        pass