import sys
import math
import numpy
import matplotlib.pyplot as plot

### Parse the calbration file
def ParseFile(InputFile, OutputList):
	f = open("./"+InputFile, "r")
	for line in f:
		templist = []
		cols = line.split("\t")
		OutputList.append(cols)
	return 0

CalibrationList = []
ParseFile("Compiled_Calibration_Parameters_Sorted_2.txt", CalibrationList)



### Create Lists
pitchsignal = []
pitchdrift = []
yawsignal = []
yawdrift = []

print CalibrationList[0]

### Separate the calibration columns to lists
for i in range(0,7807,1):
	pitchsignal.append(float(CalibrationList[i][10]))
	pitchdrift.append(float(CalibrationList[i][9]))
	
for i in range(0,7807, 1):
	yawsignal.append(float(CalibrationList[i][14]))
	yawdrift.append(float(CalibrationList[i][13]))

### Linear Fit
fitpitch = numpy.polyfit(pitchsignal, pitchdrift, 1)
fityaw = numpy.polyfit(yawsignal, yawdrift, 1)

def pitchfunction(xvalue):
	outvalue = xvalue*fitpitch[0] + fitpitch[1]
	return outvalue

def yawfunction(xvalue):
	outvalue = xvalue*fityaw[0] + fityaw[1]
	return outvalue

###############

plotyaw = []
plotpitch = []

for i in range(len(pitchsignal)):
	plotpitch.append(pitchfunction(pitchsignal[i]))
	plotyaw.append(yawfunction(yawsignal[i]))

def ParseFile(InputFile, Outputlist):
	f = open("./"+InputFile, "r")
	for line in f:
		templist =[]
		li = line.strip()
		if not li.startswith("#"):
			cols = line.split("\t")
			Outputlist.append(cols)
	return 0


print fitpitch
print fityaw


fig = plot.figure()


ax = fig.add_subplot(211)
ax.plot(pitchsignal,pitchdrift, "ro", pitchsignal, plotpitch)
ax.set_ylabel("Pitch (uradians)")
ax.set_xlabel("Pitch Signal (1/m)")

bx = fig.add_subplot(212)
bx.plot(yawsignal, yawdrift, "bo", yawsignal, plotyaw)
bx.set_ylabel("Yaw (uradians)")
bx.set_xlabel("Yaw Signal (1/m)")


plot.show()