"""
BaseSystem.py --

This file implements BatSim's Base System class.

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.

"""
import os
import vtk
import time
import thread
import putmatrix
import getmatrix
from numpy import *
from graphengine import axes
from graphengine import cindir
from systems import link
from interface import symvalues
from makegen import *

#global variables
degtorads = (pi/180)
FALSE = 0
TRUE = 1

class BaseSystem:
	""" Controlab base system's class definition. This is the main systems class"""
	def __init__( self, DH, Link1Type, Link2Type, Makegen):
		"""Initializes DH matrix and renderizes the system on the 3D screen"""
		#class parameters
		#boolean parameters.
		self.SolutionComputed  =   FALSE
		self.MakefileGenerated = FALSE

		#System type
		self.type = 'none'
		#DH parameters.
		self.alpha1 = DH[0,0]
		self.a1 = DH[0,1]
		self.theta1 = DH[0,2]
		self.d1 = DH[0,3]
		self.alpha2 = DH[1,0]
		self.a2 = DH[1,1]
		self.theta2 = DH[1,2]
		self.d2 = DH[1,3]

		#Makegen parameters
		self.systemtype=Makegen[ 0 ]
		self.odesolver	=Makegen[ 1 ]
		self.dynmodel	=Makegen[ 2 ]
		self.motor	=Makegen[ 3 ]
		self.control1	=Makegen[ 4 ]                #normal controller
		self.control2	=Makegen[ 5 ]                #swing up
		self.ctrlnum  =Makegen[ 6 ]


		#Dynamic simulation parameters.
		self.range			=pi/6 		#range
		self.swinguporder		= 0			#swing up order
		self.ctrlorder			=0			#controler order
		self.dynmodelorder		=4			#dynmodel order
		self.initime			=0.0			#tini
		self.finaltime			=4.0			#tfinal
		self.integstep			=0.001		#integrator step time
		self.ctrlstep			=0.001		#control step time

		#DYNMODEL      M    m    L       g        K    p
		self.SimParam=matrix([[0.0]*6,[0.0]*6,[0.0]*6,[0.0]*6])

		#Base Transformation Matrix
		self.BaseTrans = vtk.vtkTransform()

		#Links
		self.Link1 = link.Link( Link1Type )
		self.Link2 = link.Link( Link2Type )

		#RotCamHMT (Matrix that aligns camera to base's axes)
		self.RotCamHMT = vtk.vtkMatrix4x4()

	def 	RotCamHMTcompute( self, RotCamMat ):
		"""Fills RotCamHmt values"""
		self.RotCamHMT.SetElement(0,0, RotCamMat[ 0 ] )
		self.RotCamHMT.SetElement(0,1, RotCamMat[ 1 ] )
		self.RotCamHMT.SetElement(0,2, RotCamMat[ 2 ] )
		self.RotCamHMT.SetElement(0,3, RotCamMat[ 3 ] )
		self.RotCamHMT.SetElement(1,0, RotCamMat[ 4 ] )
		self.RotCamHMT.SetElement(1,1, RotCamMat[ 5 ] )
		self.RotCamHMT.SetElement(1,2, RotCamMat[ 6 ] )
		self.RotCamHMT.SetElement(1,3, RotCamMat[ 7 ] )
		self.RotCamHMT.SetElement(2,0, RotCamMat[ 8 ] )
		self.RotCamHMT.SetElement(2,1, RotCamMat[ 9 ] )
		self.RotCamHMT.SetElement(2,2, RotCamMat[ 10 ] )
		self.RotCamHMT.SetElement(2,3, RotCamMat[ 11 ] )
		self.RotCamHMT.SetElement(3,0, RotCamMat[ 12 ] )
		self.RotCamHMT.SetElement(3,1, RotCamMat[ 13 ] )
		self.RotCamHMT.SetElement(3,2, RotCamMat[ 14 ] )
		self.RotCamHMT.SetElement(3,3, RotCamMat[ 15 ] )

	def genericHMTcompute( self, theta1, theta2 ):
		"""Computes systems's homogeneus matrix transformation using DH"""

		#DH Parameters
		if (self.Link1.Type =="revolute"):
			self.Link1.DHvec[0,:] = [ self.alpha1, self.a1, theta1, self.d1 ]
		else:
			self.Link1.DHvec[0,:] = [ self.alpha1, self.a1, self.theta1, theta1 ]

		if (self.Link2.Type =="revolute"):
			self.Link2.DHvec[0,:] = [ self.alpha2, self.a2 , theta2, self.d2]
		else:
			self.Link2.DHvec[0,:] = [ self.alpha2, self.a2 , self.theta2, theta2]

		#HMT calculation from DH parameters.
		cindir.DHtoHMT( self.Link1.DHvec, self.Link1.HMT )
		cindir.DHtoHMT( self.Link2.DHvec, self.Link2.HMT )

		self.SystemHMT = vtk.vtkMatrix4x4()
		self.SystemHMT.Multiply4x4( self.Link1.HMT, self.Link2.HMT, self.SystemHMT)

		self.TmpHMT = vtk.vtkMatrix4x4()
		self.Link1.HMT.Multiply4x4( self.RotCamHMT, self.Link1.HMT, self.TmpHMT)
		self.Link1.HMT.DeepCopy( self.TmpHMT )
		self.Link1.Trans.SetMatrix( self.Link1.HMT )

		self.Link2.HMT.Multiply4x4( self.RotCamHMT, self.SystemHMT, self.Link2.HMT)
		self.Link2.Trans.SetMatrix( self.Link2.HMT)


	def rendering_loop(self,ren,VTKwindow):
		#simulation loop
		Timer = vtk.vtkTimerLog()
		Timer.StartTimer ()
		for i in range( (self.pt-30)/30 ):
			time.sleep( self.integstep*18 )
			self.genericHMTcompute( self.X[0, i*30], self.X[1,i*30] )
			self.Link1assembly.SetUserTransform( self.Link1.Trans )
			self.Link2assembly.SetUserTransform( self.Link2.Trans )
			VTKwindow.Render()
		Timer.StopTimer ()
		TotalTime=Timer.GetElapsedTime ()
		FPS=1#((self.pt-30)/30)/TotalTime
		print "Total visualization Time: %f  segs " % TotalTime
		print "\n FPS: %f  segs " % FPS


	def simulate( self, ren, VTKwindow, pyshell ):

		"""Cartpole's 3D dynamic simulation"""

		#Filling dynamic simulation parameters
		self.SimParam[3,0]=self.range				#range
		self.SimParam[3,1]=self.swinguporder		#swing up order
		self.SimParam[3,2]=self.ctrlorder			#controler order
		self.SimParam[3,3]=self.dynmodelorder		#dynmodel order
		self.SimParam[3,4]=self.ctrlnum			#number of controllers
		self.SimParam[2,0]=self.initime			#tini
		self.SimParam[2,1]=self.finaltime			#tfinal
		self.SimParam[2,2]=self.integstep			#integrator step time
		self.SimParam[2,3]=self.ctrlstep			#control step time
		self.SimParam[2,4]=self.grav				#Grav

		self.pt=(int)(self.SimParam[2,1]/self.SimParam[2,2]) #number of tjc points

		#call to dyncore
		Timer = vtk.vtkTimerLog()

		if self.SolutionComputed==FALSE:
			self.X=matrix([[0.0]*self.pt]*(self.SimParam[3,3]+self.SimParam[3,2]))
			#initial conditions
			if ( self.Link1.Type == "translational"):
				self.X[0,0]=self.d1
				self.X[1,0]=self.theta2
			else:
				self.X[0,0]=self.theta1
				self.X[1,0]=self.theta2

			self.timevec=matrix([0.0]*self.pt)
			self.u=matrix([0.0]*self.pt)
			Timer.StartTimer ()
			self.dyncore_call(pyshell)
			Timer.StopTimer ()
			TotalTime=Timer.GetElapsedTime ()
			print "Total dyncore Time: %f  segs " % TotalTime
			self.SolutionComputed  = TRUE
			self.MakefileGenerated = TRUE


		#starting the thread for solid rendering
		#thread.start_new_thread(self.rendering_loop,(ren,VTKwindow,))
		self.rendering_loop(ren,VTKwindow)


	def updateLinkValue( self, ren, VTKwindow, value, LinkNum ):
		"""Updates Link position and displays new data on Display Window"""
		self.SolutionComputed = FALSE
		if  LinkNum == "Link 1":
			if ( self.Link1.Type == "translational"):
				self.d1=(float)(value*0.10)
				self.genericHMTcompute( self.d1, self.theta2 )
			else:
				self.theta1=value*degtorads
				self.genericHMTcompute( self.theta1, self.theta2 )
			self.Link1assembly.SetUserTransform( self.Link1.Trans )
		else:
			if ( self.Link2.Type == "translational"):
				self.d2=(float)(value*0.10)
				self.genericHMTcompute( self.d1, self.d2 )
			else:
				self.theta2=value*degtorads
				if ( self.Link1.Type == "translational"):
					self.genericHMTcompute( self.d1, self.theta2 )
				else:
					self.genericHMTcompute( self.theta1, self.theta2 )
			self.Link2assembly.SetUserTransform( self.Link2.Trans )
		VTKwindow.Render()

	def destroy( self, ren, VTKwindow ):
		"""System's destroy method"""
		self.Systemassembly.SetVisibility(0)
		VTKwindow.Render()

	def make_compile(self,gauge,pyshell):

		makegen( self.systemtype, self.odesolver, self.dynmodel, self.motor,self.control1, self.control2, self.ctrlnum)

		gauge.SetValue(10)
		pyshell.write('Compiling\n')
		if(os.name=="nt"):
			a=os.popen('del *.o')
		else:
			a=os.popen('rm *.o')
		gauge.SetValue(20)
		b=os.popen('make cllnx')
		pyshell.write(b.read())
		gauge.SetValue(50)

	def	dyncore_call(self,pyshell):
		"""Dynamic simulation core.
		SimParam: 4x6 float matrix
		X: state variable matrix: 4Xpt,  [x,theta,dx,dtheta]
		time: time 1 x points matrix
		u: 1x points Dynamic model input matrix"""

		if(self.dyncore==0):
			pt=(int)(self.SimParam[2,1] /self.SimParam[2,2] )

			putmatrix.putmatrix(self.X[:,0],"Xini.txt")
			putmatrix.putmatrix(self.SimParam,"simparam.txt")
			#putmatrix.putmatrix(u,"u.txt")
			if(os.name=="nt"):
				os.system("dyncorec")
			else:
				os.system("./dyncorec")


			getmatrix.getmatrix(self.X,"X.txt")
			getmatrix.getmatrix(self.u,"u.txt")
		else:
			#getmatrix.getmatrix(self.X,"matlab/X.txt")


			dir=os.getcwd()
			dir = os.path.join(dir, 'matlab/'+self.systemtype)
			f=open(dir+'/'+self.matlab+'.txt','r+')
			lines=f.readlines()

			line=filter(None,lines[0].split(' '))
			self.pt=len(line)
			self.step=float(line[1])-float(line[0])
			self.finaltime=self.step*self.pt
			self.SimParam[2,1]=self.step*self.pt

			pyshell.write('From Matlab: Step = %fs ,Points = %f ,Final Time= %fs' %(self.step,self.pt,self.finaltime) )

			self.X=matrix([[0.0]*self.pt]*4)
			self.timevec=matrix([0.0]*self.pt)
			self.u=matrix([0.0]*self.pt)

			#for X1
			line=filter(None,lines[self.x1].split(' '))
			for j in range(self.pt):
				self.X[0,j]=float(line[j])

			#for X2
			line=filter(None,lines[self.x2].split(' '))
			for j in range(self.pt):
				self.X[1,j]=float(line[j])

			#for dX1
			line=filter(None,lines[self.dx1].split(' '))
			for j in range(self.pt):
				self.X[2,2]=float(line[j])

			#for dX2
			line=filter(None,lines[self.dx2].split(' '))
			for j in range(self.pt):
				self.X[3,2]=float(line[j])


		for i in range(self.pt):
			self.timevec[0,i]=i*0.001