import csv
import math
import random
import sage.plot.line
import sage.plot.plot3d.shapes2
import sage.rings.polynomial.convolution
import scipy.fftpack

def loadvectors_timestamp(csvname):
	global x_vec
	x_vec=[]
	global y_vec
	y_vec=[]
	global z_vec
	z_vec=[]
	reader=csv.reader(open(csvname),delimiter=',')
	for row in reader:
		if len(row) >= 4:
			x_vec.append(float(row[1]))
			y_vec.append(float(row[2]))
			z_vec.append(float(row[3]))
	print "sample count: ",len(x_vec)

def loadvectors_with_timestamp(csvname):
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]
	reader=csv.reader(open(csvname),delimiter=',')
	for row in reader:
		if len(row) >= 4:
			ts=float(row[0])
			x_ts_vec.append((ts,float(row[1])))
			y_ts_vec.append((ts,float(row[2])))
			z_ts_vec.append((ts,float(row[3])))
	print "sample count: ",len(x_ts_vec)

def loadagvector(csvname):
	agvector=[]
	reader=csv.reader(open(csvname),delimiter=',')
	for row in reader:
		if len(row) >= 5:
			rowlist=[float(row[0]),
				row[1],
				float(row[2]),
				float(row[3]),
				float(row[4])]
			agvector.append(rowlist)
	print "sample count: ",len(agvector)
	return agvector


def plot2lists(list1,list2):
	tuple1=zip(range(len(list1)),list1)
	tuple2=zip(range(len(list2)),list2)
	p1=sage.plot.line.line(tuple1,rgbcolor=(1,0,0))
	p2=sage.plot.line.line(tuple2,rgbcolor=(0,0,1))
	return p1+p2

def plot3lists(list1,list2,list3):
	tuple1=zip(range(len(list1)),list1)
	tuple2=zip(range(len(list2)),list2)
	tuple3=zip(range(len(list3)),list3)
	p1=sage.plot.line.line(tuple1,rgbcolor=(1,0,0))
	p2=sage.plot.line.line(tuple2,rgbcolor=(0,0,1))
	p3=sage.plot.line.line(tuple3,rgbcolor=(0,1,0))
	return p1+p2+p3

def integrate_gyro():
	global x_ts_vec
	global y_ts_vec
	global z_ts_vec
	global x_pos_vec
	x_pos_vec=[]	
	global x_vec
	x_vec=[]
	x_pos=0.0
	prevts = 0.0;
	for xs in x_ts_vec:
		ts=xs[0]
		xv=xs[1]
		x_vec.append(xv)
		if prevts>0.0:
			dt=ts-prevts
			dt=dt / 1000000000.0
			if abs(xv) < 0.06:
				xv = 0.0
			x_pos=x_pos+xv*dt
			x_pos_vec.append(x_pos)
		prevts=ts
	global y_pos_vec
	y_pos_vec=[]
	global y_vec
	y_vec=[]	
	y_pos=0.0
	prevts = 0.0;
	for ys in y_ts_vec:
		ts=ys[0]
		yv=ys[1]
		y_vec.append(yv)
		if prevts>0.0:
			dt=ts-prevts
			dt=dt / 1000000000.0
			if abs(yv) < 0.06:
				yv = 0.0
			y_pos=y_pos+yv*dt
			y_pos_vec.append(y_pos)
		prevts=ts

	global z_pos_vec
	z_pos_vec=[]
	global z_vec
	z_vec=[]
	z_pos=0.0
	prevts = 0.0;
	for zs in z_ts_vec:
		ts=zs[0]
		zv=zs[1]
		z_vec.append(zv)
		if prevts>0.0:
			dt=ts-prevts
			dt=dt / 1000000000.0
			if abs(zv) < 0.06:
				zv = 0.0
			z_pos=z_pos+zv*dt
			z_pos_vec.append(z_pos)
		prevts=ts

def getaccels(agvector):
	accels=[]
	for row in agvector:
		if(row[1]=='accel'):
			accelrow=(row[2],row[3],row[4])
			accels.append(accelrow)
	return accels

def rotz(invec,dz):
	x=invec[0]
	y=invec[1]
	z=invec[2]
	xp=x*math.cos(dz)-y*math.sin(dz)
	yp=x*math.sin(dz)+y*math.cos(dz)
	zp=z
	outvec=(xp,yp,zp)
	return outvec

def rotx(invec,dx):
	x=invec[0]
	y=invec[1]
	z=invec[2]
	xp=x
	yp=y*math.cos(dx)-z*math.sin(dx)
	zp=y*math.sin(dx)+z*math.cos(dx)
	outvec=(xp,yp,zp)
	return outvec

def roty(invec,dy):
	x=invec[0]
	y=invec[1]
	z=invec[2]
	xp=z*math.sin(dy)+x*math.cos(dy)
	yp=y
	zp=z*math.cos(dy)-x*math.sin(dy)
	outvec=(xp,yp,zp)
	return outvec

def difflimit(v1,v2):
	x1=v1[0]
	y1=v1[1]
	z1=v1[2]
	x2=v2[0]
	y2=v2[1]
	z2=v2[2]
	if abs(x1-x2)>0.1 or abs(y1-y2)>0.1 or abs(z1-z2)>0.1:
		return True
	return False

def avgvec(v1,v2):
	x1=v1[0]
	y1=v1[1]
	z1=v1[2]
	x2=v2[0]
	y2=v2[1]
	z2=v2[2]
	x=x1*0.9+x2*0.1	
	y=y1*0.9+y2*0.1
	z=z1*0.9+z2*0.1
	v=(x,y,z)
	return v

def getgyro(agvector):
	gyrovec=[]
	currentvec=()
	lastaddedvec=()
	prevts=0
	leadaccelcounter=0
	avgcurrentvec=()
	samplecounter=0
	for row in agvector:
		if samplecounter<3000:
			samplecounter=samplecounter+1
		else:
			if(row[1]=='accel' and len(currentvec)==0):
				if(len(avgcurrentvec)==0):
					avgcurrentvec=(row[2],row[3],row[4])
				else:
					if leadaccelcounter<5:
						leadaccelcounter=leadaccelcounter+1
						newvec=(row[2],row[3],row[4])
						avgcurrentvec=avgvec(avgcurrentvec,newvec)
					else:
						currentvec=avgcurrentvec
						lastaddedvec=avgcurrentvec
						gyrovec.append(currentvec)
			if(row[1]=='gyro' and len(currentvec)==3):
				if(prevts != 0 ):
					dt=row[0]-prevts
					dt=dt / 1000000000.0
					xv=row[2]
					if abs(xv) < 0.01:
						xv = 0.0
					yv=row[3]
					if abs(yv) < 0.01:
						yv = 0.0
					zv=row[4]
					if abs(zv) < 0.01:
						zv = 0.0
					dx=xv*dt
					dy=yv*dt
					dz=zv*dt
					currentvec=rotx(currentvec,-dx)
					currentvec=roty(currentvec,-dy)
					currentvec=rotz(currentvec,-dz)
					if difflimit(currentvec,lastaddedvec):
						gyrovec.append(currentvec)
						lastaddedvec=currentvec
				prevts=row[0]
	return gyrovec

def process_ag():
	global agvector
	agvector=loadagvector("agx.csv")
	global accelvector
	accelvector=getaccels(agvector)
	global gyrovector
	gyrovector=getgyro(agvector)
	return sage.plot.plot3d.shapes2.line3d(accelvector)+sage.plot.plot3d.shapes2.line3d(gyrovector,color='red')