#Write MWC, kmall watercolumn, to ascii file.
#Selects only a few datagrams between starttime and stoptime in the function
#processDatagram below.
import glob
import os
import sys
import struct
import time
import datetime

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

X1 = -9999
Y1 = -9999
Z1 = -9999

X2 = -9999
Y2 = -9999
Z2 = -9999


# Process one depth datagram, #MWC
# lenghta and chunk are from processDatagram, see below
# millisec is decoded from the header, so I send it in as a parameter here
def processWatercolumnDatagram(millisec, lenghta, chunk):
	global outfile
# Headersize is 4 bytes smaller than in the headerfile, remember that the 4
# bytes with the length has been dropped
	headersize = 1 + 1 + 1 + 1 + 1 + 1 + 2 + 4 + 4
	
	partitionsize = 2 + 2
	commonsize = 2 + 2 + 8
	common = struct.Struct('HHBBBBBBBB')
	numBytesCmnPart, pingCnt, rxFansPerPing, rxFanIndex, swathsPerPing, swathAlongPosition, \
	txTransducerInd, rxTransducerInd, numRxTransducers, algorithmType = common.unpack_from(chunk, headersize + partitionsize)
	
	EMdgmMWCtxInfo_size = 2 + 2 + 2 + 2 + 4
	EMdgmMWCtxInfo = struct.Struct('HHHhf')
	numBytesTxInfo, numTxSectors, numBytesPerTxSector, padding, heave_m = EMdgmMWCtxInfo.unpack_from(chunk, headersize + partitionsize + commonsize)

	EMdgmMWCtxSectorData_size = 4 + 4 + 4 + 2 + 2
	EMdgmMWCtxSectorData = struct.Struct('fffHh')
	start_sector_data = headersize + partitionsize + commonsize + EMdgmMWCtxInfo_size
	i = 0
	while (i < numTxSectors):
		tiltAngleReTx_deg, centreFreq_Hz, txBeamWidthAlong_deg, txSectorNum, padding = EMdgmMWCtxSectorData.unpack_from(chunk, start_sector_data)
		start_sector_data += EMdgmMWCtxSectorData_size
		i += 1

	rx_info_start = start_sector_data
	EMdgmMWCrxInfo_size = 2 + 2 + 1 + 1 + 1 + 1 + 4 + 4
	EMdgmMWCrxInfo = struct.Struct('HHBBBbff')
	numBytesRxInfo, numBeams, numBytesPerBeamEntry, phaseFlag, TVGfunctionApplied, TVGoffset_dB, sampleFreq_Hz, soundVelocity_mPerSec = EMdgmMWCrxInfo.unpack_from(chunk, rx_info_start)

	outstr = "%d\n" % (int(millisec))
	outfile.write(outstr)
	start_beamdata = rx_info_start + EMdgmMWCrxInfo_size
	EMdgmMWCrxBeamData_size = 4 + 2 + 2 + 2 + 2 + 4
	EMdgmMWCrxBeamData = struct.Struct('fHHHHf')
	one_sample = struct.Struct('b')
	i = 0
	while (i < numBeams):
		beamPointAngReVertical_deg, startRangeSampleNum, detectedRangeInSamples, beamTxSectorNum, numSampleData, detectedRangeInSamplesHighResolution = EMdgmMWCrxBeamData.unpack_from(chunk, start_beamdata)
		outstr = "%.2f " % (beamPointAngReVertical_deg)
		outfile.write(outstr)
		j = 0
		sample_start = start_beamdata + EMdgmMWCrxBeamData_size
		outstr = ""
		while (j < numSampleData):
			sample = one_sample.unpack_from(chunk, sample_start + j)[0]
			tmpstr = " %d" % (sample)
			outstr += tmpstr
			j += 1
		if (phaseFlag == 0):
			start_beamdata += EMdgmMWCrxBeamData_size + numSampleData
		elif (phaseFlag == 1):
			start_beamdata += EMdgmMWCrxBeamData_size + (2 * numSampleData)
		elif (phaseFlag == 2):
			start_beamdata += EMdgmMWCrxBeamData_size + (3 * numSampleData)
		outstr += "\n"
		outfile.write(outstr)
		i += 1


# Process one depth datagram, #MRZ, from the kmall-version from November 2019
# lenghta and chunk are from processDatagram, see below
# millisec is decoded from the header, so I send it in as a parameter here
def processDepthDatagram(millisec, lenghta, chunk):
	global outfile_dpt
	
# Headersize is 4 bytes smaller than in the headerfile, remember that the 4
# bytes with the length has been dropped
	headersize = 1 + 1 + 1 + 1 + 1 + 1 + 2 + 4 + 4
	
	partitionsize = 2 + 2
	commonsize = 2 + 2 + 8
	common = struct.Struct('HHBBBBBBBB')
	numBytesCmnPart, pingCnt, rxFansPerPing, rxFanIndex, swathsPerPing, swathAlongPosition, \
	txTransducerInd, rxTransducerInd, numRxTransducers, algorithmType = common.unpack_from(chunk, headersize + partitionsize)
	
	# Changed from version 0
	pinginfo_size = 2 + 2 + 4 + 1 + 1 + 1 + 1 + 1 + 1 + 2 + 11 * 4 + 2 + 2 + 1 + 1 + 2 + 4 + 4 + 4 + 4 + 2 + 2 + 4 + 2 + 2 + 6 * 4 + 1 + 1 + 1 + 1 + 8 + 8 + 4 + 4 + 1 + 1 + 2

	pinginfo = struct.Struct('HHfBBBBBBHfffffffffffhhBBHIfffHHfHHffffffBBBBddf')

	numBytesInfoData, padding0, pingRate_Hz, beamSpacing, depthMode,\
	subDepthMode, distanceBtwSwath, detectionMode, pulseForm, \
	padding01, frequencyMode_Hz, freqRangeLowLim_Hz, \
	freqRangeHighLim_Hz, maxTotalTxPulseLength_sec, \
	maxEffTxPulseLength_sec, maxEffTxBandWidth_Hz, \
	absCoeff_dBPerkm, portSectorEdge_deg, \
	starbSectorEdge_deg, portMeanCov_deg, \
	starbMeanCov_deg, portMeanCov_m, \
	starbMeanCov_m, modeAndStabilisation, \
	runtimeFilter1, runtimeFilter2,\
	pipeTrackingStatus, transmitArraySizeUsed_deg,\
	receiveArraySizeUsed_deg, transmitPower_dB,\
	SLrampUpTimeRemaining, padding2,\
	yawAngle_deg, numTxSectors, numBytesPerTxSector,\
	headingVessel_deg, soundSpeedAtTxDepth_mPerSec,\
	txTransducerDepth_m, z_waterLevelReRefPoint_m, \
	x_txTransducerArm_SCS_m, y_txTransducerArm_SCS_m,\
	latLongInfo, posSensorStatus, attitudeSensorStatus,\
	padding3, latitude_deg, longitude_deg,\
	ellipsoidHeightReRefPoint_m = pinginfo.unpack_from(chunk, headersize + partitionsize + commonsize)
# Bug in Python, fix it (binary alignments not correct)
	latlon = struct.Struct("d")
	klat = latlon.unpack_from(chunk, headersize + partitionsize + commonsize + 124)
	klon = latlon.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8)
	ellheight = struct.Struct("f")
	ellipsheight = ellheight.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8 + 8)
	latitude_deg = klat[0]
	longitude_deg = klon[0]
	ellipsoidHeightReRefPoint_m = ellipsheight[0]
	# Changed in Version 1
	bsCorrectionOffset_dB = ellheight.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8 + 8 + 4)[0]
	byterec = struct.Struct("B")
	lambertsLawApplied = byterec.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8 + 8 + 4 + 4)[0]
	iceWindow = byterec.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8 + 8 + 4 + 4 + 1)[0]
	shortrec = struct.Struct("H")
	padding4 = shortrec.unpack_from(chunk, headersize + partitionsize + commonsize + 124 + 8 + 8 + 4 + 4 + 1 + 1)[0]

	sec = int(millisec / 1000)
	
# Pointer offset to sectorInfo
	sectorInfo_offset = headersize + partitionsize + commonsize + pinginfo_size
	# Changed from version 0
	sectorInfo = struct.Struct('BBBBfffffffBBHfff')
	sectorInfo_size = 1 + 1 + 1 + 1 + 7 * 4 + 1 + 1 + 2 + 4 + 4 + 4
	i = 0
	while (i < numTxSectors):
		txSectorNumb, txArrNumber, txSubArray, padding0,\
		sectorTransmitDelay_sec, tiltAngleReTx_deg,\
		txNominalSourceLevel_dB, txFocusRange_m,\
		centreFreq_Hz, signalBandWidth_Hz, \
		totalSignalLength_sec, pulseShading, signalWaveForm,\
		padding1, highVoltageLevel_dB, sectorTrackingCorr_dB, effectiveSignalLength_sec = sectorInfo.unpack_from(chunk, sectorInfo_offset + i * sectorInfo_size)
		i+=1

	rxInfo_offset = sectorInfo_offset + numTxSectors * sectorInfo_size
	rxInfo = struct.Struct('HHHHffffHHHH')
	rxInfo_size = 2 + 2 + 2 + 2 + 4 + 4 + 4 + 4 + 2 + 2 + 2 + 2
	numBytesRxInfo, numSoundingsMaxMain, numSoundingsValidMain, numBytesPerSounding, \
	WCSampleRate, seabedImageSampleRate, BSnormal_dB, BSoblique_dB, \
	extraDetectionAlarmFlag, numExtraDetections, numExtraDetectionClasses, \
	numBytesPerClass = rxInfo.unpack_from(chunk, rxInfo_offset)
	
	extraDetClassInfo_offset = rxInfo_offset + rxInfo_size

	extraDetectionSize = 2 + 1 + 1
	extraDetectionStruct = struct.Struct('HBB')
		
	sounding_offset = extraDetClassInfo_offset + numExtraDetectionClasses * extraDetectionSize

	soundingStruct = struct.Struct('HBBBBBBBBHffffffHHffffffffffffffffffHHHH')
	sounding_size = 2 + 8 + 2 + 6 * 4 + 2 + 2 + 18 * 4 + 4 * 2

	outputstr = "\nPing %.8f %.8f %d %.2f %.2f %.2f %.2f %.2f %.2f %.2f\n" % (latitude_deg, longitude_deg, millisec, X1, Y1, Z1, X2, Y2, Z2, headingVessel_deg)
	outfile_dpt.write(outputstr)
	i = 0
	while(i < numSoundingsMaxMain):
		soundingIndex, txSectorNumb, detectionType, \
		detectionMethod, rejectionInfo1, rejectionInfo2, \
		postProcessingInfo, detectionClass, detectionConfidenceLevel, \
		padding, rangeFactor, qualityFactor, \
		detectionUncertaintyVer_m, detectionUncertaintyHor_m, \
		detectionWindowLength_sec, echoLength_sec, \
		WCBeamNumb, WCrange_samples, WCNomBeamAngleAcross_deg, \
		meanAbsCoeff_dBPerkm, reflectivity1_dB, reflectivity2_dB, \
		receiverSensitivityApplied_dB, sourceLevelApplied_dB, \
		BScalibration_dB, TVG_dB, beamAngleReRx_deg, \
		beamAngleCorrection_deg, twoWayTravelTime_sec, \
		twoWayTravelTimeCorrection_sec, deltaLatitude_deg, \
		deltaLongitude_deg, z_reRefPoint_m, y_reRefPoint_m, \
		x_reRefPoint_m, beamIncAngleAdj_deg, realTimeCleanInfo, \
		SIstartRange_samples, SIcentreSample, \
		SInumSamples = soundingStruct.unpack_from(chunk, sounding_offset + i * sounding_size)
		i+=1
			
# THIS IS IT.  This is where we output xyz-points
# Depths are referred to the reference point.  To get it to the waterline,
# SUBSTRACT the distance from
# Error estimates are also available: detectionUncertaintyVer_m and
# detectionUncertaintyHor_m
		waterlevel = z_reRefPoint_m - z_waterLevelReRefPoint_m
		plat = latitude_deg + deltaLatitude_deg
		plon = longitude_deg + deltaLongitude_deg
		outputstr = " %.8f %.8f %.2f %.2f %d" % (deltaLatitude_deg, deltaLongitude_deg, 
			z_reRefPoint_m, WCNomBeamAngleAcross_deg, WCrange_samples)
		outfile_dpt.write(outputstr)

def processInstallationDatagram(millisec, lenghta, chunk):
	global outfile_dpt
	global X1
	global Y1
	global Z1
	global X2
	global Y2
	global Z2

# Headersize is 4 bytes smaller than in the headerfile, remember that the 4
# bytes with the length has been dropped
	headersize = 1 + 1 + 1 + 1 + 1 + 1 + 2 + 4 + 4
	
	istruct = struct.Struct('HHH')
	istruct_size = 2 + 2 + 2
	numBytesCmnPart, info, status = istruct.unpack_from(chunk, headersize)
	# Read string
	inst = chunk[headersize + istruct_size:lenghta - headersize - istruct_size]
	istr = inst.decode("utf-8") 
	no_new_line = istr.replace("\n","")
	tokvals = no_new_line.split(',')
	no_tok = 0
	while (no_tok < len(tokvals)):
		transd = tokvals[no_tok].find("TRAI_RX")
		if (transd >= 0):
			transno = int(tokvals[no_tok][7])
			instp = tokvals[no_tok][9:].split(';')
			no_inst = 0
			while(no_inst < len(instp)):
				x = instp[no_inst].find("X=")
				if (x == 0):
					v = instp[no_inst].split('=')
					xval = float(v[1])
					if (transno == 1):
						X1 = xval
					if (transno == 2):
						X2 = xval
				y = instp[no_inst].find("Y=")
				if (y == 0):
					v = instp[no_inst].split('=')
					yval = float(v[1])
					if (transno == 1):
						Y1 = yval
					if (transno == 2):
						Y2 = yval
				z = instp[no_inst].find("Z=")
				if (z == 0):
					v = instp[no_inst].split('=')
					zval = float(v[1])
					if (transno == 1):
						Z1 = zval
					if (transno == 2):
						Z2 = zval
				no_inst += 1
		no_tok += 1
	

# What happens in processDatagram?  Read the documentation of the kmall-format.
# This is the processing of the datagram to find out what datagram type this
# is.
# The processing of each datagram type takes place in specific routines
def processDatagram(lengtha, chunk):
	header_without_length = struct.Struct('ccccBBHII')
	dgm_type0,dgm_type1,dgm_type2,dgm_type3,dgm_version,sysid,emid,sec,nsec = header_without_length.unpack_from(chunk,0)
	dgm_type = dgm_type0 + dgm_type1 + dgm_type2 + dgm_type3
		


# Decode time
	nanosec = sec
	nanosec *= 1E9
	nanosec += nsec
	millisec = nanosec
	millisec /= 1E6	

	strk = dgm_type.decode()
	
	if (dgm_version < 1 and (strk == '#MWC' or strk == '#MRZ')):
		return
	if (strk == '#IIP'):
		processInstallationDatagram(millisec, lengtha, chunk)
	# Select only datagrams within a limited time slot
	starttime = 1625650871000
	stoptime  = 1625650875000
	if (starttime <= millisec) and (millisec <= stoptime):
		if (strk == '#MWC'):
			processWatercolumnDatagram(millisec, lengtha, chunk)
		if (strk == '#MRZ'):
			processDepthDatagram(millisec, lengtha, chunk)


	
# I shall not humiliate any developer by documenting this main program.
# The processing of the datagram takes place in the routine processDatagram.
clear()
files = glob.glob('*.kmall')
for file in files:
	try:
		f = open(file, 'rb')
		nfile = file + ".wcl"
		outfile = open(nfile,'w', encoding='utf-8')
		nfile = file + ".wcldpt"
		outfile_dpt = open(nfile,'w', encoding='utf-8')
	except Exception:
		print('File',file,'not opened.')
		sys.exit(0)

	print(file)
# Process the file:
	f.seek(0, 2)
	file_size = f.tell()
	f.seek(0, 0)
	remaining = file_size

# Read all datagrams and process each of them
	while (remaining > 0):
# First read 4 bytes that contains the length of the chunk
		lenghta = struct.unpack("I",f.read(4))
		remaining -= 4
# Then read the chunk.  Note that the length read includes the 4 bytes in the
# integer.
		dgmsize = lenghta[0] - 4
		chunk = f.read(dgmsize)
		remaining -= dgmsize
# Then process this chunk
		processDatagram(dgmsize, chunk)
	
	f.close()
	outfile.close()
	outfile_dpt.close()

# If the watercolumn are in separate files, extract from kmwcd, but keep the wcldpt
files = glob.glob('*.kmwcd')
for file in files:
	try:
		f = open(file, 'rb')
		nfile = file.replace("kmwcd","kmall") + ".wcl"
		outfile = open(nfile,'w', encoding='utf-8')
	except Exception:
		print('File',file,'not opened.')
		sys.exit(0)

	print(file)
# Process the file:
	f.seek(0, 2)
	file_size = f.tell()
	f.seek(0, 0)
	remaining = file_size

# Read all datagrams and process each of them
	while (remaining > 0):
# First read 4 bytes that contains the length of the chunk
		lenghta = struct.unpack("I",f.read(4))
		remaining -= 4
# Then read the chunk.  Note that the length read includes the 4 bytes in the
# integer.
		dgmsize = lenghta[0] - 4
		chunk = f.read(dgmsize)
		remaining -= dgmsize
# Then process this chunk
		processDatagram(dgmsize, chunk)
	
	f.close()
	outfile.close()