#!/usr/bin/env python

import rospy
import mobility_games.auditory.play_wav as pw
import math
from geometry_msgs.msg import PoseStamped, Pose, Point, Vector3
from tf.transformations import euler_from_quaternion
import random
from std_msgs.msg import Header, ColorRGBA
from visualization_msgs.msg import Marker
import pyttsx
from os import system
import numpy as np 
import pyaudio
from os import system, path
import wave
from mobility_games.utils.helper_functions import angle_diff
import time 
import sys
from rospkg import RosPack
from mobility_games.cfg import OriginalGridGameConfig
from dynamic_reconfigure.server import Server

class Turns(object):

    def __init__(self):
        rospy.init_node("grid_game")
        rospy.Subscriber("/tango_pose", PoseStamped,self.process_pose)
        srv = Server(OriginalGridGameConfig, self.config_callback)
        top = RosPack().get_path('mobility_games')
        self.sound_folder = path.join(top, 'auditory/sound_files')
        self.engine = pyttsx.init()
        self.dingNoise = pw.Wav(path.join(self.sound_folder, "ding.wav"))
        self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
        self.beepNoise = pw.Wav(path.join(self.sound_folder, "beep3.wav"))
        self.beep2Noise = pw.Wav(path.join(self.sound_folder, "beep2.wav"))
        self.lastDingNoise = rospy.Time.now()
        self.lastJumpNoise = rospy.Time.now()
        self.lastBeepNoise = rospy.Time.now()
        self.lastBeep2Noise = rospy.Time.now()
        self.angleList = [90,-90,180]
        self.travel_dist = 2.0
        self.border_radius = 5.0
        self.hasSpoken = False

    def process_pose(self, msg):
        self.x = msg.pose.position.x
        self.y = msg.pose.position.y
        self.z = msg.pose.position.z
        angles = euler_from_quaternion([msg.pose.orientation.x,
                                        msg.pose.orientation.y,
                                        msg.pose.orientation.z,
                                        msg.pose.orientation.w])
        self.yaw = angles[2]

    def config_callback(self, config, level):
        self.travel_dist = config['pc_traveledDistance']
        self.border_radius = config['pc_borderRadius']

        return config
    
    #This funcation make you turn onve reach the goal distance
    def turn_game(self,angle):
        self.yaw = None  
        sound = {90:"Turn Left", -90:"Turn right", 180:"Turn around"}
        speech = sound[angle]  
        self.engine.say(speech)  #Says the turn accordng to the angle
        if not self.hasSpoken:   #Make sures that it doens't get interrupted
            self.engine.runAndWait()
            self.engine.say(speech)
            self.engine.runAndWait()
        self.hasSpoken = True
        r = rospy.Rate(10)
        while not self.yaw:  #Let us if data is coming in
            print self.yaw  
            r.sleep()
            pass
        goal_angle = self.yaw + angle*math.pi/180
        goal_angle %= 2*math.pi
        threshold = 20*math.pi/180
        while not rospy.is_shutdown():
            if abs(angle_diff(self.yaw, goal_angle)) <= threshold: #Make sure if you hit the threshhold
                self.jumpNoise.play()
                self.lastJumpNoise = rospy.Time.now()
                break  #Break from the loop
            r.sleep()

    #This function make walk in a straight line
    def straight(self, start_x, start_y):
        self.x = None
        self.y = None
        self.yaw = None
        r = rospy.Rate(10)
        while not self.yaw:  #Check to see if data is coming in
            print "NONE"
            r.sleep()
            pass
        while not self.x:  #Check to see if data is coming in
            print "None"
            r.sleep()
            pass
        straightAngle = self.yaw #the angle you are facing 
        errorAngle = 15*math.pi/180 #threshold of the angle 
        loc_x = self.x #get you location after you turn
        loc_y = self.y #get you location after you turn
        while not rospy.is_shutdown():
            #this make sure htat you stay in straight angle
            if abs(angle_diff(self.yaw, straightAngle)) < errorAngle:  #Makes sure that you are walking in a straigt line
                if rospy.Time.now() - self.lastBeepNoise > rospy.Duration(1): #Plays the beep sound every second
                    self.beepNoise.close()
                    self.beepNoise = pw.Wav(path.join(self.sound_folder, "beep3.wav"))
                    self.beepNoise.play()
                    self.lastBeepNoise = rospy.Time.now()
                x = self.x - loc_x
                y = self.y - loc_y
                dist = math.sqrt((x**2)+(y**2))
                if dist >= self.travel_dist:  #Check to see if you have the goal of 1.5 meter
                    self.dingNoise.play()
                    self.lastDingNoise = rospy.Time.now()
                    degree = random.choice(self.angleList)  #Choose a random angle from the angleList
                    if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(2):
                        self.jumpNoise.close()
                        self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
                        self.turn_game(degree)  #Enter in the turn game method
                    break
            out_x = self.x - start_x
            out_y = self.y - start_y
            outside = math.sqrt((out_x**2)+(out_y**2))
            #this check to if you out of the border
            if(outside > self.border_radius):
                self.outOfBound(start_x, start_y)
                break
            r.sleep()

    #This function make you turn if you are out of  bounds
    def outOfBound(self, start_x, start_y):
        speak = "Out of Bounds"
        self.engine.say(speak) 
        if not self.hasSpoken: 
            self.engine.runAndWait()
            self.engine.say(speak)
            self.engine.runAndWait()
        self.hasSpoken = True
        normal = [start_x - self.x, start_y - self.y] #normal vector to the start
        theta = [0, 90, 180 , -90] #all the possible angle
        best_angle = 0
        best_dotprod = -1
        for i in range(len(theta)): #this calculate al possible angle that make you go back inot the circle
            goal_angle = self.yaw + theta[i]*math.pi/180
            unit = [math.cos(goal_angle), math.sin(goal_angle)]
            dotprod = normal[0] * unit[0] + normal[1] * unit[1]
            if dotprod > best_dotprod:
                best_dotprod = dotprod
                best_angle = theta[i]
        if rospy.Time.now() - self.lastJumpNoise > rospy.Duration(2):
            self.jumpNoise.close()
            self.jumpNoise = pw.Wav(path.join(self.sound_folder, "jomp.wav"))
            self.turn_game(best_angle)
        x_1 = self.x
        y_1 = self.y
        dist = 0
        straightAngle = self.yaw
        errorAngle = 15*math.pi/180
        self.engine.say("Walk Straight")
        while dist < 1: #this make you go back inot circle for a meter
            if abs(angle_diff(self.yaw, straightAngle)) < errorAngle:  #Makes sure that you are walking in a straigt line
                if rospy.Time.now() - self.lastBeep2Noise > rospy.Duration(1): #Plays the beep sound every second
                    self.beep2Noise.close()
                    self.beep2Noise = pw.Wav(path.join(self.sound_folder, "beep2.wav"))
                    self.beep2Noise.play()
                    self.lastBeep2Noise = rospy.Time.now()
                x_2 = self.x - x_1
                y_2 = self.y - y_1
                dist = math.sqrt((x_2**2)+(y_2**2)) 



    #This function just run the program
    def run(self):
        r = rospy.Rate(10)
        self.x = None
        while not self.x:  #Check to see if data is coming in
            print "None Run Method"
            r.sleep()
            pass
        starting_pos_x = self.x #get you starting location
        starting_pos_y = self.y #git you starting location 
        while not rospy.is_shutdown():
            if rospy.Time.now() - self.lastDingNoise > rospy.Duration(2):
                self.dingNoise.close()
                self.dingNoise = pw.Wav(path.join(node.sound_folder, "ding.wav"))
                self.straight(starting_pos_x, starting_pos_y)
            r.sleep()



if __name__ == '__main__':
    node = Turns()
    node.run()