# /****************************************************************************
#  *   Copyright (c) 2019 Parker Lusk and Jesus Tordesillas Torres. All rights reserved.
#  *
#  * Redistribution and use in source and binary forms, with or without
#  * modification, are permitted provided that the following conditions
#  * are met:
#  *
#  * 1. Redistributions of source code must retain the above copyright
#  *    notice, this list of conditions and the following disclaimer.
#  * 2. Redistributions in binary form must reproduce the above copyright
#  *    notice, this list of conditions and the following disclaimer in
#  *    the documentation and/or other materials provided with the
#  *    distribution.
#  * 3. Neither the name of this repo nor the names of its contributors may
#  *    be used to endorse or promote products derived from this software
#  *    without specific prior written permission.
#  *
#  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
#  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
#  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
#  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
#  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
#  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
#  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#  * POSSIBILITY OF SUCH DAMAGE.
#  *
#  ****************************************************************************/

from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
from std_msgs.msg import ColorRGBA
import rospy
from numpy import linalg as LA
from pyquaternion import Quaternion
import numpy as np
import math
from matplotlib import cm
from tf.transformations import quaternion_from_euler
import os
GREEN=1
RED=2
BLUE=3

def getMarkerArray(color,all_pos,all_accel):
	markerArray = MarkerArray()

	skip=2
	for i in range (0,len(all_pos),skip):
		robotMarker = Marker()
		robotMarker.header.frame_id = "world"
		robotMarker.header.seq = i
		robotMarker.header.stamp    = rospy.get_rostime()
		robotMarker.ns = "robot"
		robotMarker.id = i

		robotMarker.type = robotMarker.MESH_RESOURCE
		robotMarker.action = robotMarker.ADD
		robotMarker.pose.position.x = all_pos[i][0]
		robotMarker.pose.position.y = all_pos[i][1]
		robotMarker.pose.position.z = all_pos[i][2]


		axis_z=[0,0,1]
		#all_accel gives the total accel of the quad, including gravity // Accel_total =  accel_motors - 9.81

		accel=[ all_accel[i][0],  all_accel[i][1],  all_accel[i][2]+9.81]  #This is the accel produced by the motors

		if(LA.norm(accel)>0.001 and LA.norm(np.cross(accel, axis_z))>0.0001):
			accel=accel/LA.norm(accel)
			axis=np.cross(accel, axis_z);
			axis=axis/LA.norm(axis)

			dot=np.dot(accel,axis_z)
			angle=math.acos(dot)        
			my_quaternion = Quaternion(axis=axis, angle=-angle)

			robotMarker.pose.orientation.x = my_quaternion.elements[1]
			robotMarker.pose.orientation.y = my_quaternion.elements[2]
			robotMarker.pose.orientation.z = my_quaternion.elements[3]
			robotMarker.pose.orientation.w = my_quaternion.elements[0]
		elif (accel[2]<0): #Upside down
			my_quaternion = Quaternion(axis=np.array([1,0,0]), angle=-math.pi)

			robotMarker.pose.orientation.x = my_quaternion.elements[1]
			robotMarker.pose.orientation.y = my_quaternion.elements[2]
			robotMarker.pose.orientation.z = my_quaternion.elements[3]
			robotMarker.pose.orientation.w = my_quaternion.elements[0]

		else: #Hover position
			robotMarker.pose.orientation.x = 0
			robotMarker.pose.orientation.y = 0
			robotMarker.pose.orientation.z = 0
			robotMarker.pose.orientation.w = 1

		robotMarker.scale.x = 1.0
		robotMarker.scale.y = 1.0
		robotMarker.scale.z = 1.0

		robotMarker.lifetime = rospy.Duration()
         
   		if(color==RED):
   			robotMarker.color=ColorRGBA(1, 0, 0, 1)
   		if(color==GREEN):
   			robotMarker.color=ColorRGBA(0,1, 0, 1)
  		if(color==BLUE):
			robotMarker.color=ColorRGBA(0, 0, 1, 1)

		color=cm.jet(int(((i*1.0)/len(all_pos)*256)));
		robotMarker.color=ColorRGBA(color[0], color[1], color[2], color[3])
		robotMarker.mesh_resource = "package://project/models/quad.stl";

		markerArray.markers.append(robotMarker) 

	return markerArray


# def spawnWindowInGazebo(x,y,z,roll,pitch,yaw):
# 	os.system("rosrun gazebo_ros spawn_model -file `rospack find acl_sim`/urdf/window.urdf -urdf -x " + str(x) + " -y " + str(y) + " -z " + str(z) + " -R " + str(roll) + " -P " + str(pitch) + " -Y " + str(yaw)+ " -model gate")
# 	pass

def getMarkerWindow(x,y,z,r,p,yaw):

	myMarker = Marker()
	myMarker.header.frame_id = "world"
	myMarker.header.seq = 1
	myMarker.header.stamp    = rospy.get_rostime()
	myMarker.ns = "window"
	myMarker.id = 1
	myMarker.type = myMarker.MESH_RESOURCE # sphere
	myMarker.action = myMarker.ADD
	myMarker.pose.position.x = x
	myMarker.pose.position.y = y
	myMarker.pose.position.z = z
	q = quaternion_from_euler(r, p, yaw)
	myMarker.pose.orientation.x=q[0]
	myMarker.pose.orientation.y=q[1]
	myMarker.pose.orientation.z=q[2]
	myMarker.pose.orientation.w=q[3]
	myMarker.mesh_resource = "package://project/models/window_buena.stl";
	myMarker.color=ColorRGBA(0, 1, 0, 1)
	myMarker.scale.x = 5;
	myMarker.scale.y = 5;
	myMarker.scale.z = 6;

	return myMarker