#!/usr/bin/env python
# Wave motion service for wavedemo

from time import sleep
import sys
import numpy as np
import rospy
from geometry_msgs.msg import Point
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from std_srvs.srv import Trigger, TriggerResponse
import matplotlib.pyplot as plt
import signal

class NextageWave:

    def __init__(self):

        self.names = [u'RARM_JOINT0', u'RARM_JOINT1', u'RARM_JOINT2', u'RARM_JOINT3', u'RARM_JOINT4', u'RARM_JOINT5']

        # Wave trajectory message, poses and timing information
        self.initpos = np.zeros(6)
        self.wave1 = np.array([4.02507408e-04, -8.42815834e-01, -3.76086535e-01, -8.06545915e-05, -2.06423466e+00, 3.17927198e-04])
        self.wave2 = np.array([4.02507408e-04, -8.42815834e-01, -3.76086535e-01, -8.06545915e-05, -2.06423466e+00, -1.0])
        self.wavepos = [self.wave1, self.wave2, self.wave1, self.wave2, self.wave1, self.initpos]
        self.timing = [2.0, 2.5, 3.0, 3.5, 4.0, 6.0]
        self.trajectory_time = self.timing[len(self.timing)-1]
        
        self.trajectory_message = JointTrajectory()
        self.trajectory_message.joint_names = [x.encode('ascii') for x in self.names]
        for i in range(0, len(self.wavepos)):
            point = JointTrajectoryPoint()
            point.velocities = np.zeros(6).tolist()
            point.positions = self.wavepos[i].tolist()
            point.time_from_start = rospy.Duration(self.timing[i])
            self.trajectory_message.points.append(point)

        # Home trajectory message
        self.home_message = JointTrajectory()
        self.home_message.joint_names = self.trajectory_message.joint_names
        point = JointTrajectoryPoint()
        point.positions = self.initpos 
        point.time_from_start = rospy.Duration(5.0)
        self.home_message.points.append(point)

        # Wave/Home trajectory service setups
        self.service = rospy.Service("wave_service", Trigger, self.trigger_response)
        self.service = rospy.Service("home_service", Trigger, self.trigger_home)
        self.pub = rospy.Publisher("/nextagea/rarm_controller/command", JointTrajectory, queue_size=1)
        self.boolTrigger = False

    def spin(self):
        while not rospy.is_shutdown():
            try:
                # If wave service initalised, trigger wave
                if self.boolTrigger == True:
                    print("[wave] Triggering wave")
                    self.trajectory_message.header.stamp = rospy.Time.now()
                    self.pub.publish(self.trajectory_message)
                    self.boolTrigger = False
                sleep(0.1)
            except KeyboardInterrupt:
                break


    def trigger_home(self, request):
        print("[wave] Triggering homing - robot going to init pos")
        self.home_message.header.stamp = rospy.Time.now()
        self.pub.publish(self.home_message)
        sleep(5.0)
        return TriggerResponse(success=True, message="Done")

    def trigger_response(self, request):
        self.boolTrigger = True
        sleep(self.trajectory_time)
        return TriggerResponse(success=True, message="Wave initiated")

def sig_int_handler(signal, frame):
    raise KeyboardInterrupt

def main(args):
    rospy.init_node("wave_service")
    signal.signal(signal.SIGINT, sig_int_handler)
    wave = NextageWave()
    wave.spin()

if __name__ == '__main__':
    main(sys.argv)
