#Read .wcl and .wcldpt and create .csv for watercolumn
import glob
import os
import sys
import struct
import time
import datetime
import utm
import math

clear = lambda: os.system('cls')
outfile = 0

refpos = []
positions = []
times = []
angs = []
dpts = []
noBeams = []
samp = []
heading = []

X1 = -99999
Y1 = -99999
Z1 = -99999
X2 = -99999
Y2 = -99999
Z2 = -99999


def readDpt(f):
	global X1
	global Y1
	global Z1
	global X2
	global Y2
	global Z2

	line = f.readline()
	while(line):
		toks = line.split()
		if (len(toks) < 2):
			line = f.readline()
			continue
		if (toks[0] == "Ping"):
			lat = float(toks[1])
			lon = float(toks[2])
			refpos.append(lat)
			refpos.append(lon)
			tim = int(toks[3])
			times.append(tim)
			X1 = float(toks[4])
			Y1 = float(toks[5])
			Z1 = float(toks[6])
			X2 = float(toks[7])
			Y2 = float(toks[8])
			Z2 = float(toks[9])
			head_deg = float(toks[10])
			heading.append(head_deg)
		else:
			notoks = len(toks)
			noBeams.append(notoks / 5)
			i = 0
			while (i < notoks):
				alat = lat + float(toks[i])
				alon = lon + float(toks[i + 1])
				dpt = float(toks[i + 2])
				ang = float(toks[i + 3])
				detsamp = int(toks[i + 4])
				angs.append(ang)
				dpts.append(dpt)
				positions.append(alat)
				positions.append(alon)
				samp.append(detsamp)
				i += 5
		line = f.readline()

	
def makeCSV(f, csv):
	global positions
	global refpos
	global times
	global angs
	global dpts
	global noBeams
	
	# transducer position 
	prev_trans = ""

	line = f.readline()
	linecnt = 0
	while(line):
		linecnt += 1
		modline = linecnt % 20
		if (modline == 0):
			print("linecnt: %d" % (linecnt))
		toks = line.split()
		if (len(toks) == 1):
			tim = int(toks[0])
			print(tim)
		else:
			bang = float(toks[0])
			# The rest are samples
			# Now make xyz
			timIdx = -1
			i = 0
			while(i < len(times)):
				if (times[i] == tim):
					break
				i += 1
			if (i < len(times)):
				timIdx = i
			if (timIdx < 0):
				return # serious error, cannot happen
			no_beams = noBeams[timIdx]
			# Beams are sorted
			startdpts = 0
			j = 0
			while(j != timIdx):
				startdpts += noBeams[j]
				j += 1
			kmknjjh = int(startdpts)
			startdpts = kmknjjh #force startdpts to be int
			refp_lat = refpos[timIdx * 2]
			refp_lon = refpos[(timIdx * 2) + 1]
			u = utm.from_latlon(refp_lat, refp_lon)
			rx = u[0]
			ry = u[1]
			hdg = heading[timIdx]
			r = -9999
			alfa = -9999
			if (abs(Y1) < 0.02):
				r = X1
				if (r < 0):
					alfa = 180
				else:
					alfa = 0
			if (abs(X1) < 0.02):
				r = Y1
				if (r < 0):
					alfa = 270
				else:
					alfa = 90
			if (X1 == 0.0 and Y1 == 0.0):
				r = 0
				alfa = 0
			if (r == -9999):
				r = math.sqrt(X1 * X1 + Y1 * Y1)
				alfa = math.degrees(math.atan(X1/Y1))

			hdg += alfa
			cir = 450.0 - hdg
			hx = math.sin(math.radians(cir)) * r
			hy = math.cos(math.radians(cir)) * r
			rx += hy
			ry += hx
			
			# position of transducer
			transpos = "%.2f %.2f %.2f\n" % (rx, ry, Z1*-1.0)
			if (prev_trans != transpos):
				trans.write(transpos)
				prev_trans = transpos

			# From startdepths, find the beam closest to this angle
			angdiff = 90
			angidx = -1
			k = 0
			while (k < no_beams):
				adiff = abs(bang - angs[startdpts + k])
				if (adiff < angdiff):
					angdiff = adiff
					angidx = k
				k += 1
			bdpt = dpts[startdpts + angidx]


			beam_lat = positions[(startdpts + angidx) * 2]
			beam_lon = positions[((startdpts + angidx) * 2) + 1]
			u = utm.from_latlon(beam_lat, beam_lon)
			bx = u[0]
			by = u[1]
			
			detsamp = float(samp[startdpts + angidx])

			downw = bdpt - Z1
			
			x_diff = bx - rx
			y_diff = by - ry

			x_inc = x_diff / detsamp
			y_inc = y_diff / detsamp
			z_inc = downw / detsamp

			cnt = 0
			noSamplesInBeam = len(toks) - 1
			while(cnt < noSamplesInBeam):
				idxsamp = cnt + 1
				val = int(toks[idxsamp])
				xsamp = rx + (x_inc * (cnt + 1))
				ysamp = ry + (y_inc * (cnt + 1))
				zsamp = ((z_inc * (cnt + 1)) * -1.0) - Z1
				outstr = "%.2f %.2f %.2f %d\n" % (xsamp, ysamp, zsamp, val)
				csv.write(outstr)
				cnt += 1

		line = f.readline()


# I shall not humiliate any developer by documenting this main program.
clear()
files = glob.glob('*.wcl')
for file in files:
	try:
		f = open(file, 'r')
		nfile = file + "dpt"
		df = open(nfile,'r')
		nfile = file + ".csv"
		csv = open(nfile,'w', encoding='utf-8')
		nfile = file + ".transducer.csv"
		trans = open(nfile,'w', encoding='utf-8')
	except Exception:
		print('File',file,'not opened.')
		sys.exit(0)

	print(file)

	# Read the whole dpt-file into RAM, should not be too big.
	positions.clear()
	refpos.clear()
	noBeams.clear()
	times.clear()
	angs.clear()
	dpts.clear()
	samp.clear()
	heading.clear()
	readDpt(df)

	# Then process the watercolumn file
	makeCSV(f, csv)

	f.close()
	df.close()
	csv.close()
	trans.close()