#!/usr/bin/python
"""
	Linkages
	
"""
import argparse
import numpy as np
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib import cm		# color maps

#to draw lines in matplotlib
from matplotlib.path import Path
import matplotlib.patches as patches



def coupling_curve(l1,l2,l3,l4,l5, alpha=np.pi/2, theta=0, act=2, theta1=0):
	"""
	li : segmets are length of linkage Arms [cm]
	theta : angular position of driving actuator [rad] 
	act :  define which actuator is driving actuator [2,3,4]
	theta1 : angle of base frame [rad]
	"""
	
	def theta2(theta):
		M1 = (l1**2 + l2**2 + l3**2 - l4**2)/(l2*l3)
		M2 = l1/l2
		M3 = l1/l3
		H = M3**2*np.cos(theta)**2 + (-2*M1*M3+2*M2)*np.cos(theta) + (M1**2 - M2**2 -1)
		#print H
		exp_itheta3 = (M1 - M3*np.cos(theta) - np.lib.scimath.sqrt(H))/(M2-(np.exp(-1j*theta)))
		#exp_itheta32 = (M1 - M3*np.cos(theta) + np.sqrt(H))/(M2-(np.exp(-1j*theta)))

		#try is line, trying to deal with the +/- in the eqtn
		#exp_itheta3 = exp_itheta3 + exp_itheta32

		exp_itheta2 = np.exp(1j*theta)
		return exp_itheta2, exp_itheta3

	def theta3():
		# get to this later when other stuff works
		return "blank"

	def theta4():
		# get to this later when other stuff works
		return "blank"

	actuator = { 2 : theta2, 
						3: theta3,
						4: theta4,
	}

	try:
		exp_itheta2, exp_itheta3 = actuator[act](theta)
	except:
		print "Yikes Unexpected error:", sys.exc_info()[0]

	P = l2*exp_itheta2*np.exp(1j*theta1) + l5*np.exp(1j*alpha)*exp_itheta3*np.exp(1j*theta1)


	return P, exp_itheta2, exp_itheta3

def main(args):
	l1 = .25
	l2 = .15
	l3 = .15
	l4 = .15
	l5 = .15
	alpha = -np.pi/3.
	theta = np.arange(0, 2.*np.pi, np.pi/10)
	th1 = 0
	
	# find position of endpoint of coupling linkage
	Point, eth2, eth3 = coupling_curve(l1, l2, l3, l4, l5, alpha, theta, 2, th1)
	# convert to angles
	th2 = np.angle(eth2)
	th3 = np.angle(eth3)
	a = np.sin(th2)
	b = np.cos(th2) + l1/l2
	c = (l1/l4)*np.cos(th2) + (l1**2+l2**2+l4**2-l3**2)/(2*l2*l4)
	th4 = 2*np.arctan((a + np.lib.scimath.sqrt(a**2 + b**2-c**2))/(b+c))




	p = Point[~np.isnan(Point).any(axis=0)]
	#print p
	# draw lines of linkage
	#def drawlines():
	picangle = 1*len(th2)/4.
	l3orig = [l2*np.cos(th2[picangle]), l2*np.sin(th2[picangle])]
	l4end = [(l1*np.cos(th1) + l4*np.cos(th4[picangle])), (l1*np.sin(th1)+l4*np.sin(th4[picangle]))]

	print  l4end


	verts = [
			(0.,0.),
			(l3orig[0], l3orig[1]),
			(l4end[0], l4end[1]),
			(l1*np.cos(th1), l1*np.sin(th1)),
			(0.,0.),
			(l3orig[0], l3orig[1]),
			(l3orig[0]+l5*np.cos(alpha), l3orig[1]+l5*np.sin(alpha)),
			]

	codes = [
			Path.MOVETO,
			Path.LINETO,
			Path.LINETO,
			Path.LINETO,
			Path.LINETO,
			Path.MOVETO,
			Path.LINETO,
			]
	path = Path(verts, codes)
	
	#print l3end[0]
	#return path



	
	

	fig = plt.figure()
	ax = fig.add_subplot(111)
	
	patch = patches.PathPatch(path, facecolor='orange', lw=2)
	ax.add_patch(patch)
	Pmag = np.absolute(Point)
	Px = Pmag*np.cos(np.angle(Point))
	Py = Pmag*np.sin(np.angle(Point))

	plt.plot(Point.real, np.imag(Point))
	plt.show()

	# 15.1a Plot Rosenblock function
	#fig = plt.figure()
	#ax = fig.add_subplot(projection='3d')
	#ax = Axes3D(fig)
	#ax.plot_surface(x,y,f, cmap=cm.summer)
	#ax.contour(x,y,f, zdir='z', offset=-10,cmap=cm.summer, vmax=2500)
	


	
	#plt.show()




if __name__ == '__main__':
	parser = argparse.ArgumentParser()
	parser.add_argument("-n", "--n", type=int, default=100, help="number of points")
	parser.add_argument("-s", "--s", type=int, default=0.5, help="standard deviation")
	args = parser.parse_args()
	main(args)