#!/usr/bin/env python

import roslib
import rospy
import math
import actionlib
import numpy as np
import random
import trajectory_msgs.msg as tm
from std_msgs.msg import String, Float32
from enum import Enum
from std_srvs.srv import Empty, EmptyResponse

    


class Swing:
    def __init__(self):
        self.dis_sub = rospy.Subscriber("goal_distance", Float32, self.distance_callback)

        self.joint_pub = rospy.Publisher('arm_controller/command', tm.JointTrajectory, queue_size=10)
        self.joint_names = rospy.get_param("arm_controller/joints")
        print(self.joint_names)

        self.ready_srv = rospy.Service("ready", Empty, self.ready_callback)
        self.up_srv = rospy.Service("upswing", Empty, self.upswing_callback)
        self.up_srv = rospy.Service("hit", Empty, self.hit_callback)

        self.goal_pos = np.zeros(len(self.joint_names))
        self.joint_vel = np.zeros(len(self.joint_names))
        self.duration = 1.5
        self.move = True

        self.curr_dis = 0.0

    def distance_callback(self, data):
        rospy.loginfo("get distance: %s", data)
        self.curr_dis = data.data/1000.0

    def ready_callback(self, emp):
        self.goal_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 3.14]
        #self.joint_vel = [0.0, 0.0, 0.0, 0.0, 0.0, 0.5]
        print("ready")

        return EmptyResponse()

    def upswing_callback(self, emp):
        self.goal_pos = [-1.8, 0.0, 0.0, 0.0, 0.0, 3.14]
        print("up")
        return EmptyResponse()

    def hit_callback(self, emp):
        self.goal_pos = [1.8, 0.0, 0.0, 0.0, 0.0, 3.14]
        print("hit")
        v = math.sqrt(self.curr_dis/math.sin(2*0.174533))
        omega = v/(0.39225+0.425)
        self.duration = 1.5*(3.6/omega)
        rospy.loginfo("hitting duration: %s", str(self.duration))
        # self.duration = 0.7
        return EmptyResponse()

    def move_to_pose(self):

        jt = tm.JointTrajectory()
        jt.joint_names = self.joint_names
        jt.header.stamp = rospy.Time.now()


        jtp = tm.JointTrajectoryPoint()
        jtp.positions = self.goal_pos
        jtp.velocities = self.joint_vel
        jtp.accelerations = np.zeros(len(self.joint_names))
        jtp.time_from_start = rospy.Duration(self.duration)
        jt.points.append(jtp)


        self.joint_pub.publish(jt)


        self.move = False



def main():
    """ The main() function. """
    #ros node init

    rospy.init_node('swing')
    r = rospy.Rate(10)

    swing = Swing()

    while not rospy.is_shutdown():
        swing.move_to_pose()
        r.sleep()



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