#!/usr/bin/env python

# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020

@author: Maxi
"""

import re
import roslib
import rospy
import sys

import std_msgs.msg as msg
from sensor_msgs.msg import JointState
from dynamic_reconfigure.server import Server as DynamicReconfigureServer

# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType

class Monitor:
    def __init__(self):
        self.statusOK = True
        self.counter = 0
        """ status """
        self.rotation = 0
        self.elevation = 'stowed'
        self.elev_angle = 90  # deg
        self.servo = 0

        """ ROS setup """        
        rospy.init_node('Monitor')
        self.pub = rospy.Publisher('monitor',
                        msg.String,
                        queue_size=5)
        self.InterruptPub = rospy.Publisher('interrupt',
                            msg.String,
                            queue_size=3)
        self.pub_gs_monitor = rospy.Publisher('groundstation_monitoring',
						    JointState,
						    queue_size=1)
        self.listener_gs_cmd = rospy.Subscriber('groundstation_command',
						    JointState,
						    callback=self.callback_cmd)
        self.listener_gs_monitor = rospy.Subscriber('groundstation_monitoring',
						    JointState,
						    callback=self.callback_mon)
        self.listener_ch = rospy.Subscriber('chatter',
                            node_example,
                            callback=self.callback_custom_msg)
        self.listener_st = rospy.Subscriber('stowed_switch',
                            msg.String,
                            callback=self.callback_generic,
                            callback_args='stowed_switch')
        self.listener_ra = rospy.Subscriber('raised_switch',
                            msg.String,
                            callback=self.callback_generic,
                            callback_args='raised_switch')
        self.listener_lim = rospy.Subscriber('limit_switch',
                            msg.String,
                            callback=self.callback_generic,
                            callback_args='limit_switch')
        self.listener_test = rospy.Subscriber('tests',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='tests')
        self.listener_int = rospy.Subscriber('interrupt',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='interrupt')
        self.listener_cmd = rospy.Subscriber('command',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='command')
        self.listener_forw = rospy.Subscriber('forward_switch',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='forward_switch')
        self.listener_rot = rospy.Subscriber('rotation',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='rotation')
        self.listener_elev = rospy.Subscriber('elevation',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='elevation')
        self.listener_servo = rospy.Subscriber('servo',
                              msg.String,
                              callback=self.callback_generic,
                              callback_args='servo')
        while not rospy.is_shutdown():
            self.enable_activities()
            rospy.spin()
            # time.sleep(0.1)

        

    def callback_generic(self, data, topic):
        rospy.loginfo('%s: sent me %s', topic, data.data)
        if topic == 'rotation':
            self.rotation = float(data.data)
        elif topic == 'elevation':
            if 'STATE' in data.data:
                self.elevation = re.sub('STATE: ', '', data.data)
                rospy.loginfo(self.elevation)
            elif 'ANGLE' in data.data:
                self.elev_angle = int(re.findall('-*[0-9]+', data.data)[0])
                rospy.loginfo(self.elev_angle)
        elif topic == 'servo':
            self.servo = float(data.data)
        if self.counter > 10:
            self.enable_activities()
            self.send_status()
            self.counter = 0
        self.counter += 1
                      
    def callback_custom_msg(self, data):
        rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
                  data.message)

    def callback_cmd(self, 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)

    def callback_mon(self, cmd):
        rospy.loginfo(rospy.get_caller_id() + 'Status was sent \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)

    def enable_activities(self):
        if abs(self.rotation) < 1.5:
            self.InterruptPub.publish(msg.String('ELEVATE: ENABLE'))
        else:
            self.InterruptPub.publish(msg.String('ELEVATE: DISABLE'))

        if self.elevation == 'raised':
            self.InterruptPub.publish(msg.String('ROTATE: ENABLE'))
            self.InterruptPub.publish(msg.String('SERVO: ENABLE'))
        else:
            self.InterruptPub.publish(msg.String('ROTATE: DISABLE'))
            self.InterruptPub.publish(msg.String('SERVO: DISABLE'))

    def send_status(self):
        msg = JointState()
        msg.name = ['J1_turntable', 'J2_fold', 'J3_nod']
        msg.position = [self.deg_to_rad(self.rotation),
                        self.deg_to_rad(self.elev_angle),
                        self.deg_to_rad(self.servo)]
        msg.velocity = []
        msg.effort = []
        self.pub_gs_monitor.publish(msg)

    def deg_to_rad(self, degrees):
        rad = degrees * 3.1415 / 180.0
        return rad

    def elev_to_rad(self, state):
        if state == 'raised':
            return 0.0
        elif state == 'stowed':
            return deg_to_rad(90)
        elif state == 'tilted':
            return deg_to_rad(-30)
        else:
            return 
        
            

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