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 vecdiff(v1,v2):
	dx=v1[0]-v2[0]
	dy=v1[1]-v2[1]
	dz=v1[2]-v2[2]
	v=(dx,dy,dz)
	return v

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 absvec(v1):
	absv=math.sqrt( (v1[0]*v1[0])+(v1[1]*v1[1])+(v1[2]*v1[2]))
	return absv 

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 getcompensated(agvector,gravity):
	global errorvec
	errorvec=[]
	global abserrorvec
	abserrorvec=[]
	global diffvec
	diffvec=[]
	gyrovec=[]
	simvec=()
	currentvec=()
	lastaddedvec=()
	prevts=0
	leadaccelcounter=0
	avgcurrentvec=()
	samplecounter=0
	g_min=gravity*0.95
	g_max=gravity*1.05
	for row in agvector:
		samplecounter=samplecounter+1
		if(samplecounter>3000):
			if(row[1]=='accel'):
				amp=math.sqrt( (row[2]*row[2])+(row[3]*row[3])+(row[4]*row[4]))
				accelvec=(row[2],row[3],row[4])
				diff=()
				if len(simvec)==3:
					diff=vecdiff(accelvec,simvec)
					diffvec.append(diff)
				if amp>g_min and amp<g_max:
					if len(diff)==3:
						errorvec.append(diff)
						abserr=absvec(diff)
						abserrorvec.append((samplecounter,abserr))
					currentvec=accelvec
					lastaddedvec=currentvec
					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)
					simvec=currentvec
					if difflimit(currentvec,lastaddedvec):
						gyrovec.append(currentvec)
						lastaddedvec=currentvec
				prevts=row[0]
	return gyrovec

def calculate_gravity_accel(agvector):
	accelsum=0
	samplecounter=0
	for row in agvector:
		if row[1]=='accel':
			ampl=math.sqrt( (row[2]*row[2])+(row[3]*row[3])+(row[4]*row[4]) )
			accelsum=accelsum+ampl
			samplecounter=samplecounter+1
			if samplecounter>=10:
				return accelsum / samplecounter
	return -1

def calculate_accel_vector_abs(agvector):
	absvector=[]
	samplecounter=0
	for row in agvector:
		samplecounter=samplecounter+1
		if row[1]=='accel':
			ampl=math.sqrt( (row[2]*row[2])+(row[3]*row[3])+(row[4]*row[4]) )
			absvector.append((samplecounter,ampl))
	return absvector

def process_turn45degree():
	global x_vec
	x_vec=[]
	global y_vec
	y_vec=[]
	global z_vec
	z_vec=[]	
	loadvectors_timestamp("../measurements/turn45degree.csv")

def process_accel_railrotation1():
	global x_vec
	x_vec=[]
	global y_vec
	y_vec=[]
	global z_vec
	z_vec=[]	
	loadvectors_with_timestamp("../measurements/accel_railrotations1.csv")

def process_gyro_railrotation1():
	global x_vec
	x_vec=[]
	global y_vec
	y_vec=[]
	global z_vec
	z_vec=[]	
	loadvectors_timestamp("../measurements/gyro_railrotations1.csv")

def process_abs_gyro_turn45degree():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/turn45degree.csv")
	integrate_gyro()


def process_abs_gyro_railrotation1():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/gyro_railrotations1.csv")
	integrate_gyro()

def process_abs_gyro_90clockwise():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/z90degreeclockwise.csv")
	integrate_gyro()

def process_abs_gyro_90anticlockwise():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/z90degreeanticlockwise.csv")
	integrate_gyro()

def process_abs_gyro_180clockwise():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/z180degreeclockwise.csv")
	integrate_gyro()

def process_abs_gyro_360clockwise():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/z360degreeclockwise.csv")
	integrate_gyro()

def process_abs_gyro_noise():
	global x_ts_vec
	x_ts_vec=[]
	global y_ts_vec
	y_ts_vec=[]
	global z_ts_vec
	z_ts_vec=[]	
	loadvectors_with_timestamp("../measurements/gyronoise.csv")
	integrate_gyro()


def plot_3vec_range(xmin,xmax):
	return plot3lists(x_vec[xmin:xmax],y_vec[xmin:xmax],z_vec[xmin:xmax])

def plot_comp_accel():
	return sage.plot.plot3d.shapes2.line3d(compensatedvector,color='red')+sage.plot.plot3d.shapes2.line3d(accelvector)

def process_ag(csvname):
	global agvector
	agvector=loadagvector(csvname)
	gravity=calculate_gravity_accel(agvector)
	print "Gravity: ",gravity
	global absvector
	absvector=calculate_accel_vector_abs(agvector)
	global accelvector
	accelvector=getaccels(agvector)
	global gyrovector
	gyrovector=getgyro(agvector)
	global compensatedvector
	compensatedvector=getcompensated(agvector,gravity)

def process_ag_swing_fast():
	process_ag("capture.csv")