
/** @file wls_subspace_processing.c
 *
 * @brief This file contains source code to calculate fine timing using
 * sub-space methods (ESPRIT)
 *
 *  Usage:
 *
 *
 * Copyright 2023 NXP
 *
 * NXP CONFIDENTIAL
 * The source code contained or described herein and all documents related to
 * the source code (Materials) are owned by NXP, its
 * suppliers and/or its licensors. Title to the Materials remains with NXP,
 * its suppliers and/or its licensors. The Materials contain
 * trade secrets and proprietary and confidential information of NXP, its
 * suppliers and/or its licensors. The Materials are protected by worldwide
 * copyright and trade secret laws and treaty provisions. No part of the
 * Materials may be used, copied, reproduced, modified, published, uploaded,
 * posted, transmitted, distributed, or disclosed in any way without NXP's prior
 * express written permission.
 *
 * No license under any patent, copyright, trade secret or other intellectual
 * property right is granted to or conferred upon you by disclosure or delivery
 * of the Materials, either expressly, by implication, inducement, estoppel or
 * otherwise. Any license under such intellectual property rights must be
 * express and approved by NXP in writing.
 *
 */

/************************************************************************
 * DFW Source code to calculate fine timing using sub-space methods (ESPRIT)
 ************************************************************************/

// Standard includes.
#include <stdio.h>
//#include <stdlib.h>
#include <string.h>
#include <math.h>
// 4 sqrtf, 1 atan2f, 1 atanf, 2 floorf
#ifdef ARM_DS5
#include <arm_neon.h>
#endif

// Specific includes.
#include "wls_subspace_processing.h"
#include "wls_param_defines.h"
#include "wls_QR_algorithm.h"
#include "wls_radix4Fft.h"

#define FFT_INPUT_SIZE_MAX 32
#define FFT_INPUT_SIZE_SHORT_WINDOW 16
#define FFT_INPUT_SIZE_LONG_WINDOW 32

#ifdef STA_20_ONLY
#define BUFFER_OFFSET (12 * MAX_FFT_SIZE / 2)
#else
#define BUFFER_OFFSET (3 * MAX_FFT_SIZE / 2)
#endif
#define SUBSP_DIM_L_MAX MAX_MAT_SIZE
// needs to be floor(0.75*SUB_DIM_L)
#define SIG_SUBSP_DIM_MAX 15 // 7

#define MUSIC_THRESH_REL_5GHz 0.01f
#define MUSIC_THRESH_REL_2GHz 0.1f
#define MUSIC_THRESH_MIN 0.02f
#define MUSIC_THRESH_MAX 0.10f

#define SUB_DET_THRESH_5G_80MHZ 0.1f
#define SUB_DET_THRESH_5G_40MHZ 0.08f
#define SUB_DET_THRESH_5G_20MHZ 0.04f // 0.1f
#define SUB_DET_THRESH_20MHZ 0.253f
#define SUB_DET_THRESH_Legacy 0.38f
#define SUB_DET_THRESH_REL_5G_80MHZ 0.01f
#define SUB_DET_THRESH_REL_5G_40MHZ 0.025f
#define SUB_DET_THRESH_REL_5G_20MHZ 0.005f
#define SUB_DET_THRESH_REL_2G 0.015f
#define SUB_DET_THRESH_REL_Legacy 0.25f

#define SUB_DIM_L_6 6
#define SUB_DIM_L_10 10
#define SUB_DIM_L_12 12
#define SUB_DIM_L_16 16
#define SUB_DIM_L_20 20
#define SUB_DIM_L_22 22

#define USE_TLS
#define FFT_TD_OSF_SHIFT 1
#define FFT_FD_EDGE 0

void calcCorMatrix(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer,
		   unsigned int *totalpower, int firstPathDelay,
		   float *realCorrMatrix, unsigned int *scratchMemory,
		   int subsp_dim_L, int fft_input_size, int FD_downsample,
		   int *numObsPtr)
{
	int ii, jj, kk, ll, rowIdx, colIdx, qq;
	int loadOffset;
	float *fftTempBuffer = (float *)
		scratchMemory; //[2*(FFT_INPUT_SIZE_MAX<<FFT_TD_OSF_SHIFT)];
	float *strPtr, *strPtrTr, *ldPtr, *ldPtrTr;
	// float debugBuffer[FFT_INPUT_SIZE_MAX];
	float *corrMatrix =
		(float *)scratchMemory +
		2 * (FFT_INPUT_SIZE_MAX
		     << FFT_TD_OSF_SHIFT); // size:
					   // [SUBSP_DIM_L_MAX*2*SUBSP_DIM_L_MAX];
	unsigned int *loadPtr;
	unsigned int tempVal, loadOffsetU32, loadOffsetMask;
	short tempI, tempQ;
	float valI, valQ, valTempI, valTempQ, scale;
	float corrI, corrQ, sumI, sumQ;
	float *tempBuffer = (float *)scratchMemory +
			    2 * (FFT_INPUT_SIZE_MAX << FFT_TD_OSF_SHIFT) +
			    2 * SUBSP_DIM_L_MAX *
				    SUBSP_DIM_L_MAX; // [2*FFT_INPUT_SIZE_MAX];
	int storeIdx, loadIdx;

	int nRx = pktinfo->nRx + 1;
	int nTx = pktinfo->nTx + 1;
	int ifftSizeOsf = 1 << (pktinfo->fftSize + 6);

	int firstPathIndex = (firstPathDelay + (1 << (TOA_FPATH_BIPT - 1))) >>
			     TOA_FPATH_BIPT;
	int windowStart =
		firstPathIndex -
		(fft_input_size << (IFFT_OSF_SHIFT - 1)); // center window
							  // middle on
							  // first-path

	int stepsize = 1 << (IFFT_OSF_SHIFT - FFT_TD_OSF_SHIFT);
	int numObs = fft_input_size - subsp_dim_L - 2 * FFT_FD_EDGE;

	memset((void *)corrMatrix, 0,
	       2 * SUBSP_DIM_L_MAX * SUBSP_DIM_L_MAX * sizeof(float));
	loadOffsetMask = ifftSizeOsf * NUM_PARALLEL - 1;

	for (ii = 0; ii < nTx; ii++) {
		for (jj = 0; jj < nRx; jj += NUM_PARALLEL) {
			for (qq = 0; qq < NUM_PARALLEL; qq++) {
				loadPtr = fftOutBuffer +
					  (jj + ii * nRx) * ifftSizeOsf + qq;
				loadOffset = windowStart * NUM_PARALLEL;
				scale = SQRTF(1.0f * nTx * nRx *
					      totalpower[(jj + qq) + ii * nRx] /
					      (1 << 15)) /
					(1 << (8 + FFT_TD_OSF_SHIFT)); // 32.31
								       // format
				strPtr = fftTempBuffer;
				for (kk = 0;
				     kk < (fft_input_size << FFT_TD_OSF_SHIFT) *
						  FD_downsample;
				     kk++) {
					loadOffsetU32 =
						loadOffset & loadOffsetMask;
					tempVal = loadPtr[loadOffsetU32];
					loadOffset += stepsize * NUM_PARALLEL;
					tempI = tempVal & 0xffff;
					tempQ = (tempVal >> 16) & 0xffff;

					valI = tempI * scale;
					*strPtr++ = valI;
					valQ = tempQ * scale;
					*strPtr++ = valQ;
				}
				radix2FftFlt(fftTempBuffer,
					     (fft_input_size
					      << FFT_TD_OSF_SHIFT) *
						     FD_downsample,
					     twiddleTableFlt, MAX_FFT_FLT);
				if (FD_downsample == 4) {
					for (ll = 1; ll < fft_input_size;
					     ll++) {
						fftTempBuffer[2 *
							      ll] = fftTempBuffer
							[2 * FD_downsample * ll];
						fftTempBuffer[2 * ll +
							      1] = fftTempBuffer
							[2 * FD_downsample * ll +
							 1];
					}
				}
				// diagonal
				// rowIdx = 0
				ldPtr = fftTempBuffer +
					2 * (FFT_FD_EDGE + 1); // skip 0
				strPtr = corrMatrix;
				sumI = 0;
				storeIdx = 0;
				for (kk = 0; kk < numObs; kk++) {
					valI = *ldPtr++; // fftTempBuffer[2 *
							 // kk];
					valQ = *ldPtr++; // fftTempBuffer[2 * kk
							 // + 1];
					corrI = valI * valI + valQ * valQ;
					sumI += corrI;
					tempBuffer[storeIdx++] = corrI;
				}
				*strPtr += sumI; // corrMatrix[0]
				strPtr += 2 * (SUBSP_DIM_L_MAX + 1);
				loadIdx = 0;
				for (rowIdx = 1; rowIdx < subsp_dim_L;
				     rowIdx++) {
					sumI -= tempBuffer[loadIdx++];
					valI = *ldPtr++; // fftTempBuffer[2 *
							 // (rowIdx-1 + kk)];
					valQ = *ldPtr++; // fftTempBuffer[2 *
							 // (rowIdx-1 + kk) +
							 // 1];
					corrI = valI * valI + valQ * valQ;
					sumI += corrI;
					tempBuffer[storeIdx++] = corrI;
					*strPtr += sumI; // corrMatrix[rowIdx *
							 // 2 *
							 // (SUBSP_DIM_L_MAX+1)]
							 // += sumI;
					strPtr += 2 * (SUBSP_DIM_L_MAX + 1);
				}
				// off-diagonals
				for (colIdx = 1; colIdx < subsp_dim_L;
				     colIdx++) {
					// rowIdx = 0
					ldPtr = fftTempBuffer +
						2 * (FFT_FD_EDGE + 1);
					ldPtrTr = fftTempBuffer +
						  2 * (FFT_FD_EDGE + 1) +
						  2 * colIdx;
					strPtr = corrMatrix +
						 2 * colIdx * SUBSP_DIM_L_MAX;
					sumI = 0;
					sumQ = 0;
					storeIdx = 0;
					for (kk = 0; kk < numObs; kk++) {
						valI = *ldPtrTr++; // fftTempBuffer[2
								   // * (colIdx +
								   // kk)];
						valQ = *ldPtrTr++; // fftTempBuffer[2
								   // * (colIdx +
								   // kk) + 1];

						valTempI = *ldPtr++; // fftTempBuffer[2
								     // * kk];
						valTempQ = *ldPtr++; // fftTempBuffer[2
								     // * kk +
								     // 1];

						corrI = valI * valTempI +
							valQ * valTempQ;
						corrQ = valQ * valTempI -
							valI * valTempQ;

						sumI += corrI;
						sumQ += corrQ;

						tempBuffer[storeIdx++] = corrI;
						tempBuffer[storeIdx++] = corrQ;
					}
					*strPtr++ += sumI; // corrMatrix[2 *
							   // colIdx *
							   // SUBSP_DIM_L_MAX]
					*strPtr++ -= sumQ; // corrMatrix[2 *
							   // colIdx *
							   // SUBSP_DIM_L_MAX +
							   // 1]
					strPtr += 2 * SUBSP_DIM_L_MAX;
					loadIdx = 0;
					for (rowIdx = 1;
					     rowIdx < subsp_dim_L - colIdx;
					     rowIdx++) {
						sumI -= tempBuffer[loadIdx++];
						sumQ -= tempBuffer[loadIdx++];

						valI = *ldPtrTr++; // fftTempBuffer[2
								   // * (rowIdx -
								   // 1 + colIdx
								   // + kk)];
						valQ = *ldPtrTr++; // fftTempBuffer[2
								   // * (rowIdx -
								   // 1 + colIdx
								   // + kk) + 1];

						valTempI = *ldPtr++; // fftTempBuffer[2
								     // * (rowIdx
								     // - 1 +
								     // kk)];
						valTempQ = *ldPtr++; // fftTempBuffer[2
								     // * (rowIdx
								     // - 1 + kk)
								     // + 1];

						corrI = valI * valTempI +
							valQ * valTempQ;
						corrQ = valQ * valTempI -
							valI * valTempQ;

						sumI += corrI;
						sumQ += corrQ;

						tempBuffer[storeIdx++] = corrI;
						tempBuffer[storeIdx++] = corrQ;

						*strPtr++ += sumI; // corrMatrix[(rowIdx
								   // + colIdx) *
								   // 2 *
								   // SUBSP_DIM_L_MAX
								   // + 2 *
								   // rowIdx]
						*strPtr++ -= sumQ; // corrMatrix[(rowIdx
								   // + colIdx) *
								   // 2 *
								   // SUBSP_DIM_L_MAX
								   // + 2 *
								   // rowIdx + 1]
						strPtr += 2 * SUBSP_DIM_L_MAX;
					}
				}
			}
		}
	}
	for (rowIdx = 0; rowIdx < subsp_dim_L; rowIdx++) {
		ldPtr = corrMatrix + (rowIdx * 2 * SUBSP_DIM_L_MAX);
		strPtrTr = corrMatrix + (2 * rowIdx);
		for (colIdx = 0; colIdx < rowIdx; colIdx++) {
			corrI = *ldPtr++; // use symmetry
			corrQ = *ldPtr++;

			*strPtrTr++ = corrI;
			*strPtrTr = -corrQ;
			strPtrTr += 2 * SUBSP_DIM_L_MAX - 1;
		}
	}

	// create real matrix
	for (rowIdx = 0; rowIdx < subsp_dim_L / 2; rowIdx++) {
		ldPtr = corrMatrix + (rowIdx * 2 * SUBSP_DIM_L_MAX);
		ldPtrTr = corrMatrix +
			  (((subsp_dim_L - 1) - rowIdx) * 2 * SUBSP_DIM_L_MAX +
			   2 * (subsp_dim_L - 1) + 1);
		strPtr = realCorrMatrix + (rowIdx * SUBSP_DIM_L_MAX);
		strPtrTr = realCorrMatrix +
			   (rowIdx + subsp_dim_L / 2) * SUBSP_DIM_L_MAX;
		for (colIdx = 0; colIdx < subsp_dim_L / 2; colIdx++) {
			valI = *ldPtr++; // R00
			valQ = *ldPtr++;

			corrQ = *ldPtrTr--;
			corrI = *ldPtrTr--; // JR11J
			valTempI = valI + corrI;
			valTempQ = valQ - corrQ;

			// Ax = real(myR00+conj(myJR11J));
			// Bx = imag(myR00+conj(myJR11J));
			strPtr[subsp_dim_L / 2] = valTempQ; // Bx
			*strPtr++ = valTempI; // Ax
			strPtrTr[subsp_dim_L / 2] = valTempI; // Ax
			*strPtrTr++ = -valTempQ; // -Bx
		}
	}
	for (rowIdx = 0; rowIdx < subsp_dim_L / 2; rowIdx++) {
		ldPtr = corrMatrix +
			(((subsp_dim_L - 1) - rowIdx) * 2 * SUBSP_DIM_L_MAX);
		ldPtrTr =
			corrMatrix +
			((subsp_dim_L - 1) * 2 * SUBSP_DIM_L_MAX + 2 * rowIdx);
		strPtr = realCorrMatrix + (rowIdx * SUBSP_DIM_L_MAX);
		strPtrTr = realCorrMatrix +
			   (rowIdx + subsp_dim_L / 2) * SUBSP_DIM_L_MAX;
		for (colIdx = 0; colIdx < subsp_dim_L / 2; colIdx++) {
			valI = *ldPtrTr++; // R01J
			valQ = *ldPtrTr;
			ldPtrTr -= 2 * SUBSP_DIM_L_MAX + 1;

			corrI = *ldPtr++; // R01J.'
			corrQ = *ldPtr++;
			valTempI = valI + corrI;
			valTempQ = valQ + corrQ;
			// Cx = real(myR01J+conj(myR01J'));
			// Dx = imag(myR01J+conj(myR01J'));
			strPtr[subsp_dim_L / 2] += valTempQ; // Dx
			*strPtr++ += valTempI; // Cx
			strPtrTr[subsp_dim_L / 2] -= valTempI; // -Cx
			*strPtrTr++ += valTempQ; // Dx
		}
	}
	*numObsPtr = numObs;

	// scale input by 1/((fft_input_size-SUBSP_DIM_L+1)*nRx*nTx) - no
}

int findSigSpace(float *inputBuff, float *eMatrix, float thresh, int matSizeN,
		 hal_pktinfo_t *pktinfo, float sub_det_thresh_rel)
{
	int ii, jj;
	float *eigVals = inputBuff;
	float *Qmat = inputBuff + SUBSP_DIM_L_MAX;
	float *tempMatSum; // tempMatSum[SUBSP_DIM_L*SUBSP_DIM_L];
	float *tempMatDiff; // tempMatDiff[SUBSP_DIM_L*SUBSP_DIM_L];
	float max;
	int maxIdx;
	int sigSpaceDim = 0;

	// find max for thresh
	max = 0;
	for (jj = 0; jj < matSizeN; jj++) {
		if (eigVals[jj] > max) {
			max = eigVals[jj];
		}
	}

	if (sub_det_thresh_rel * max > thresh)
		thresh = sub_det_thresh_rel * max;

	for (ii = 0; ii < SIG_SUBSP_DIM_MAX; ii++) {
		// sort
		max = 0;
		maxIdx = -1;
		for (jj = 0; jj < matSizeN; jj++) {
			if (eigVals[jj] > max) {
				max = eigVals[jj];
				maxIdx = jj;
			}
		}
		if (max > thresh) {
			eigVals[maxIdx] = 0;
			// copy eigVec
			for (jj = 0; jj < matSizeN; jj++) {
				eMatrix[jj + sigSpaceDim * matSizeN] =
					Qmat[jj + maxIdx * SUBSP_DIM_L_MAX];
			}
			sigSpaceDim++;
		}
	}

	// if (sigSpaceDim > 1){
	tempMatSum = (float *)inputBuff; // tempMatSum[SUBSP_DIM_L*SUBSP_DIM_L];
	// tempMatDiff = (float*) inputBuff+SUBSP_DIM_L*SUBSP_DIM_L; //
	// tempMatDiff[SUBSP_DIM_L*SUBSP_DIM_L]
	tempMatDiff =
		(float *)inputBuff +
		SUBSP_DIM_L_MAX *
			SUBSP_DIM_L_MAX; // tempMatDiff[SUBSP_DIM_L*SUBSP_DIM_L]
	for (ii = 0; ii < sigSpaceDim; ii++) {
		for (jj = 1; jj < matSizeN; jj++) {
			tempMatSum[jj - 1 + ii * matSizeN] =
				eMatrix[jj - 1 + ii * matSizeN] +
				eMatrix[jj + ii * matSizeN];
			tempMatDiff[jj - 1 + ii * matSizeN] =
				eMatrix[jj - 1 + ii * matSizeN] -
				eMatrix[jj + ii * matSizeN];
		}
		tempMatSum[jj - 1 + ii * matSizeN] =
			SQRTF(2) * eMatrix[matSizeN / 2 - 1 + ii * matSizeN];
		tempMatDiff[jj - 1 + ii * matSizeN] =
			-SQRTF(2) * eMatrix[matSizeN - 1 + ii * matSizeN];
	}

	for (ii = 0; ii < sigSpaceDim; ii++) {
		for (jj = 0; jj < matSizeN / 2 - 1; jj++) {
			// E_lower
			eMatrix[jj + ii * SUBSP_DIM_L_MAX] =
				tempMatSum[jj + ii * matSizeN];
			// E_upper
			eMatrix[jj + (ii + sigSpaceDim) * SUBSP_DIM_L_MAX] =
				-tempMatDiff[(jj + matSizeN / 2) +
					     ii * matSizeN];
		}
		// E_lower
		eMatrix[jj + ii * SUBSP_DIM_L_MAX] =
			tempMatSum[matSizeN - 1 + ii * matSizeN];
		// E_upper
		eMatrix[jj + (ii + sigSpaceDim) * SUBSP_DIM_L_MAX] =
			tempMatDiff[matSizeN - 1 + ii * matSizeN];

		for (jj++; jj < matSizeN - 1; jj++) {
			// E_lower
			eMatrix[jj + ii * SUBSP_DIM_L_MAX] =
				tempMatSum[jj + ii * matSizeN];
			// E_upper
			eMatrix[jj + (ii + sigSpaceDim) * SUBSP_DIM_L_MAX] =
				tempMatDiff[(jj - matSizeN / 2) + ii * matSizeN];
		}
		for (; jj < SUBSP_DIM_L_MAX; jj++) {
			eMatrix[jj + ii * SUBSP_DIM_L_MAX] = 0;
			eMatrix[jj + (ii + sigSpaceDim) * SUBSP_DIM_L_MAX] = 0;
		}
	}
	return sigSpaceDim;
}

void calcDimOnePhase(float *eMatrix, float *omega_hat_sort, int matSizeN)
{
	int ii;
	float sumI, sumQ, omega_hat_pos;
	// float* tempVecReal = eMatrix + SUBSP_DIM_L; //
	// tempVecReal[SUBSP_DIM_L]; float* tempVecImag = eMatrix +
	// 2*SUBSP_DIM_L; // tempVecImag[SUBSP_DIM_L];
	float *tempVecReal =
		eMatrix + SUBSP_DIM_L_MAX; // tempVecReal[SUBSP_DIM_L];
	float *tempVecImag =
		eMatrix + 2 * SUBSP_DIM_L_MAX; // tempVecImag[SUBSP_DIM_L];
	float *V1_real, *V2_Real, *V1_imag, *V2_imag;

	// copy real eigen vectors
	for (ii = 0; ii < matSizeN / 2; ii++) {
		tempVecReal[ii] = eMatrix[ii];
		tempVecReal[ii + matSizeN / 2] = eMatrix[matSizeN / 2 - 1 - ii];
		tempVecImag[ii] = eMatrix[ii + matSizeN / 2];
		tempVecImag[ii + matSizeN / 2] = -eMatrix[matSizeN - 1 - ii];
	}

	V1_real = tempVecReal;
	V2_Real = tempVecReal + 1;
	V1_imag = tempVecImag;
	V2_imag = tempVecImag + 1;

	sumI = 0.0f;
	sumQ = 0.0f;
	for (ii = 0; ii < matSizeN - 1; ii++) {
		sumI += V1_real[ii] * V2_Real[ii] + V1_imag[ii] * V2_imag[ii];
		sumQ += V1_real[ii] * V2_imag[ii] - V1_imag[ii] * V2_Real[ii];
	}

	omega_hat_pos = -1 * atan2f(sumQ, sumI);
	omega_hat_sort[0] =
		(omega_hat_pos > 0) ? omega_hat_pos : (omega_hat_pos + 2 * PI);
}

void calc_QpTimesE(float *eMatrix, float *qMatrix, float *qPeMatrix,
		   int matSizeN, int matSizeM)
{
	int ii, jj, kk;
	float sum;

	for (ii = 0; ii < matSizeM; ii++) {
		for (jj = 0; jj < matSizeN; jj++) {
			sum = 0;
			for (kk = 0; kk < matSizeN; kk++) {
				sum += qMatrix[kk + jj * SUBSP_DIM_L_MAX] *
				       eMatrix[kk + ii * SUBSP_DIM_L_MAX];
			}
			qPeMatrix[jj + ii * SUBSP_DIM_L_MAX] = sum;
		}
	}
}

void constructVmatrix(float *eigValPlusVec, float *vMatrix, int sigSpaceDim,
		      int matSizeN)
{
	int ii, jj;
	float *eigVals = eigValPlusVec;
	float *Qmat = eigValPlusVec + SUBSP_DIM_L_MAX;

	float min;
	int minIdx;

	for (ii = 0; ii < sigSpaceDim; ii++) {
		// sort
		min = 1e3f;
		minIdx = -1;
		for (jj = 0; jj < matSizeN; jj++) {
			if (eigVals[jj] < min) {
				min = eigVals[jj];
				minIdx = jj;
			}
		}
		eigVals[minIdx] = 1e3f;
		// copy eigVec
		for (jj = 0; jj < sigSpaceDim; jj++) {
			// V upper
			vMatrix[ii + jj * SUBSP_DIM_L_MAX] =
				-Qmat[jj + minIdx * SUBSP_DIM_L_MAX];
		}
		for (; jj < matSizeN; jj++) {
			// V lower
			vMatrix[ii + jj * SUBSP_DIM_L_MAX] =
				Qmat[jj + minIdx * SUBSP_DIM_L_MAX];
		}
	}
}

void calcEtimesE(float *eMatrix, float *ePeMatrix, int matSizeN, int matSizeM)
{
	int ii, jj, kk;
#ifdef ARM_DS5
	float32x2_t resVec0, resVec1, resVec2, resVec3, strVec0, strVec1;
	float32x2_t rowVec0, rowVec1, colVec0, colVec1;
	float32x2x2_t tempVec0; //, tempVec1;

	for (ii = 0; ii < matSizeM; ii += 2) {
		for (jj = 0; jj <= ii; jj += 2) {
			resVec0 = vdup_n_f32(0.0f);
			resVec1 = vdup_n_f32(0.0f);
			resVec2 = vdup_n_f32(0.0f);
			resVec3 = vdup_n_f32(0.0f);
			for (kk = 0; kk < matSizeN; kk += 2) {
				rowVec0 = vld1_f32((
					float32_t const
						*)(eMatrix +
						   (kk + jj * SUBSP_DIM_L_MAX)));
				rowVec1 = vld1_f32(
					(float32_t const
						 *)(eMatrix +
						    (kk +
						     (jj + 1) *
							     SUBSP_DIM_L_MAX)));

				colVec0 = vld1_f32((
					float32_t const
						*)(eMatrix +
						   (kk + ii * SUBSP_DIM_L_MAX)));
				colVec1 = vld1_f32(
					(float32_t const
						 *)(eMatrix +
						    (kk +
						     (ii + 1) *
							     SUBSP_DIM_L_MAX)));

				resVec0 = vmla_f32(resVec0, rowVec0, colVec0);
				resVec1 = vmla_f32(resVec1, rowVec1, colVec0);
				resVec2 = vmla_f32(resVec2, rowVec0, colVec1);
				resVec3 = vmla_f32(resVec3, rowVec1, colVec1);
			}
			strVec0 = vpadd_f32(resVec0, resVec1);
			strVec1 = vpadd_f32(resVec2, resVec3);

			vst1_f32((float32_t *)(ePeMatrix + jj +
					       ii * SUBSP_DIM_L_MAX),
				 strVec0); // VST1.32 {d0}, [r0]
			vst1_f32((float32_t *)(ePeMatrix + jj +
					       (ii + 1) * SUBSP_DIM_L_MAX),
				 strVec1);
			// use symmetry
			tempVec0 = vzip_f32(strVec0, strVec1); // VZIP.32 d0,d0
			vst1_f32((float32_t *)(ePeMatrix + ii +
					       jj * SUBSP_DIM_L_MAX),
				 tempVec0.val[0]); // VST1.32 {d0}, [r0]
			vst1_f32((float32_t *)(ePeMatrix + ii +
					       (jj + 1) * SUBSP_DIM_L_MAX),
				 tempVec0.val[1]);
		}
	}
#else
	float sum, sum1, sum2, sum3;

	for (ii = 0; ii < matSizeM; ii += 2) {
		for (jj = 0; jj < matSizeM; jj += 2) {
			sum = 0;
			sum1 = 0;
			sum2 = 0;
			sum3 = 0;
			for (kk = 0; kk < matSizeN; kk += 2) {
				sum += eMatrix[kk + jj * SUBSP_DIM_L_MAX] *
				       eMatrix[kk + ii * SUBSP_DIM_L_MAX];
				sum1 += eMatrix[kk + (jj + 1) * SUBSP_DIM_L_MAX] *
					eMatrix[kk + ii * SUBSP_DIM_L_MAX];
				sum2 += eMatrix[kk + jj * SUBSP_DIM_L_MAX] *
					eMatrix[kk + (ii + 1) * SUBSP_DIM_L_MAX];
				sum3 += eMatrix[kk + (jj + 1) * SUBSP_DIM_L_MAX] *
					eMatrix[kk + (ii + 1) * SUBSP_DIM_L_MAX];
				sum += eMatrix[(kk + 1) + jj * SUBSP_DIM_L_MAX] *
				       eMatrix[(kk + 1) + ii * SUBSP_DIM_L_MAX];
				sum1 += eMatrix[(kk + 1) +
						(jj + 1) * SUBSP_DIM_L_MAX] *
					eMatrix[(kk + 1) + ii * SUBSP_DIM_L_MAX];
				sum2 += eMatrix[(kk + 1) + jj * SUBSP_DIM_L_MAX] *
					eMatrix[(kk + 1) +
						(ii + 1) * SUBSP_DIM_L_MAX];
				sum3 += eMatrix[(kk + 1) +
						(jj + 1) * SUBSP_DIM_L_MAX] *
					eMatrix[(kk + 1) +
						(ii + 1) * SUBSP_DIM_L_MAX];
			}
			// if (kk < matSizeN) { // odd matSizeN case
			//	sum += eMatrix[kk + jj*SUBSP_DIM_L_MAX] *
			//eMatrix[kk + ii*SUBSP_DIM_L_MAX]; 	sum1 += eMatrix[kk +
			//(jj + 1)*SUBSP_DIM_L_MAX] * eMatrix[kk +
			//ii*SUBSP_DIM_L_MAX]; 	sum2 += eMatrix[kk +
			//jj*SUBSP_DIM_L_MAX] * eMatrix[kk + (ii +
			//1)*SUBSP_DIM_L_MAX]; 	sum3 += eMatrix[kk + (jj +
			//1)*SUBSP_DIM_L_MAX] * eMatrix[kk + (ii +
			//1)*SUBSP_DIM_L_MAX];
			//}
			ePeMatrix[jj + ii * SUBSP_DIM_L_MAX] = sum;
			ePeMatrix[jj + 1 + ii * SUBSP_DIM_L_MAX] = sum1;
			ePeMatrix[jj + (ii + 1) * SUBSP_DIM_L_MAX] = sum2;
			ePeMatrix[(jj + 1) + (ii + 1) * SUBSP_DIM_L_MAX] = sum3;
		}
	}
#endif
}

int calcPhase(float *psiHatMatrix, int sigSpaceDim, float *omega_hat_sort)
{
	int ii, jj;
	// float omega_hat[SUBSP_DIM_L];
	float omega_hat[SUBSP_DIM_L_MAX];
	float tempStr;

	for (ii = 0; ii < sigSpaceDim; ii++) {
		omega_hat[ii] = -2 * atanf(psiHatMatrix[ii]);
	}
	// sort
	for (ii = 0; ii < sigSpaceDim; ii++) {
		omega_hat_sort[ii] = (omega_hat[ii] > 0) ?
					     omega_hat[ii] :
					     (omega_hat[ii] + 2 * PI);
		for (jj = ii; jj > 0; jj--) {
			if (omega_hat_sort[jj] < omega_hat_sort[jj - 1]) {
				tempStr = omega_hat_sort[jj - 1];
				omega_hat_sort[jj - 1] = omega_hat_sort[jj];
				omega_hat_sort[jj] = tempStr;
			}
		}
	}
	// remove double eigen values (from complex conjugate pairs)
	jj = 0;
	for (ii = 1; ii < sigSpaceDim; ii++) {
		if (omega_hat_sort[ii] - omega_hat_sort[jj] > 1e-2f) {
			jj++;
			tempStr = omega_hat_sort[ii];
			omega_hat_sort[jj] = tempStr;
		}
	}
	sigSpaceDim -= (ii - 1 - jj);

	return sigSpaceDim;
}

void calcDelayAmplitude(hal_pktinfo_t *pktinfo, unsigned int *fftOutBuffer,
			unsigned int *totalpower, int firstPathDelay,
			int sigSpaceDim, float *omega_hat_sort, float *tau_hat,
			float *aMatrix, int fft_input_size)
{
	int ii, jj, kk, qq;
	int index;
	unsigned int *loadPtr;
	unsigned int tempLoad;
	short tempI, tempQ;
	float tempVal, scale, delta;
	float leftI, leftQ, rightI, rightQ;
	float leftSqr, rightSqr, valSqr;
	// float fracIndex[SUBSP_DIM_L];
	float fracIndex[SUBSP_DIM_L_MAX];

	int nRx = pktinfo->nRx + 1;
	int nTx = pktinfo->nTx + 1;
	int ifftSizeOsf = 1 << (pktinfo->fftSize + 6);
	int signalBandwidth = 20 << (pktinfo->sigBw);
	int stepsize = 1 << IFFT_OSF_SHIFT;

	int firstPathIndex = (firstPathDelay + (1 << (TOA_FPATH_BIPT - 1))) >>
			     TOA_FPATH_BIPT;
	int windowStart =
		firstPathIndex -
		(fft_input_size << (IFFT_OSF_SHIFT - 1)); // center window
							  // middle on
							  // first-path

	unsigned int bitMask = NUM_PARALLEL * ifftSizeOsf - 1;

	// map phase to delay
	for (kk = 0; kk < sigSpaceDim; kk++) {
		tempVal = omega_hat_sort[kk] / (2 * PI);
		fracIndex[kk] = tempVal * (fft_input_size * stepsize);
		tempVal *= fft_input_size / (1.0f * signalBandwidth);
		// windowStart = (windowStart+2)&0xfffffffc;
		tempVal += windowStart /
			   (1.0f * stepsize * signalBandwidth); // windowstat in
								// oversampled
								// samples
		tau_hat[kk] = tempVal;
	}

	// project delays onto time domain data to estimate path amplitude
	for (ii = 0; ii < nTx; ii++) {
		for (jj = 0; jj < nRx; jj += NUM_PARALLEL) {
			for (qq = 0; qq < NUM_PARALLEL; qq++) {
				loadPtr = fftOutBuffer +
					  (jj + ii * nRx) * ifftSizeOsf + qq;
				scale = SQRTF(1.0f * nTx * nRx *
					      totalpower[(jj + qq) + ii * nRx] /
					      (1 << 15)) /
					(1 << 8); // 32.31 format
				// scale =
				// SQRTF(1.0f/fixedpower[jj+ii*nRx]/(1<<15))/(1<<8);
				// // 32.31 format scale =
				// SQRTF(1.0f/fixedpower[jj+ii*nRx]/(1<<0))/(1<<3);
				// // 32.31 format scale = 1;
				for (kk = 0; kk < sigSpaceDim; kk++) {
					index = (int)floorf(fracIndex[kk]);
					delta = fracIndex[kk] - index;
					index += windowStart;

					// treatment for negative windowStart??
					// if(index<0) ...

					tempLoad =
						loadPtr[(NUM_PARALLEL * index) &
							bitMask];
					tempI = tempLoad & 0xffff;
					tempQ = (tempLoad >> 16) & 0xffff;
					leftI = tempI * scale;
					leftQ = tempQ * scale;
					leftSqr = leftI * leftI + leftQ * leftQ;

					tempLoad = loadPtr[(NUM_PARALLEL *
							    (index + 1)) &
							   bitMask];
					tempI = tempLoad & 0xffff;
					tempQ = (tempLoad >> 16) & 0xffff;
					rightI = tempI * scale;
					rightQ = tempQ * scale;
					rightSqr = rightI * rightI +
						   rightQ * rightQ;

					valSqr = (rightSqr - leftSqr) * delta +
						 leftSqr;

					aMatrix[kk + (jj + qq) * sigSpaceDim +
						ii * nRx * sigSpaceDim] =
						valSqr;
				}
			}
		}
	}
}

int determineFirstPath(hal_pktinfo_t *pktinfo, float *tau_hat, float *aMatrix,
		       int sigSpaceDim, float music_thresh_rel)
{
	int ii, jj, kk;
	int nRx = pktinfo->nRx + 1;
	int nTx = pktinfo->nTx + 1;
	int firstPathIdx;
	int maxIdx = -1;
	int firstPathDelay = 0;

	float detThresh;
	float maxVal = 0;

	// float sumPower[SUBSP_DIM_L];
	float sumPower[SUBSP_DIM_L_MAX];

	int signalBandwidth = 20 << (pktinfo->sigBw);
	int stepsize = 1 << IFFT_OSF_SHIFT;

	for (kk = 0; kk < sigSpaceDim; kk++) {
		sumPower[kk] = 0;
	}

	for (ii = 0; ii < nTx; ii++) {
		for (jj = 0; jj < nRx; jj++) {
			for (kk = 0; kk < sigSpaceDim; kk++) {
				sumPower[kk] += aMatrix[kk + jj * sigSpaceDim +
							ii * nRx * sigSpaceDim];
			}
		}
	}

	// find max
	for (kk = 0; kk < sigSpaceDim; kk++) {
		sumPower[kk] /= (nTx * nRx);
		if (sumPower[kk] > maxVal) {
			maxVal = sumPower[kk];
			maxIdx = kk;
		}
	}

	detThresh = maxVal * music_thresh_rel;
	if (detThresh > MUSIC_THRESH_MAX) {
		detThresh = MUSIC_THRESH_MAX;
	} else if (detThresh < MUSIC_THRESH_MIN) {
		detThresh = MUSIC_THRESH_MIN;
	}
	// detThresh = maxVal/4;
	firstPathIdx = maxIdx;
	for (kk = maxIdx - 1; kk >= 0; kk--) {
		if (sumPower[kk] > detThresh) {
			firstPathIdx = kk;
		}
	}
	firstPathDelay = (int)floorf((tau_hat[firstPathIdx] * signalBandwidth *
				      stepsize * (1 << TOA_FPATH_BIPT)) +
				     0.5f);

	return firstPathDelay;
}

int calcSubspaceFineTiming(hal_pktinfo_t *pktinfo, // structure with CSI buffer
						   // parameters
			   unsigned int *fftOutBuffer, // buffer holding
						       // time-domain CSI
			   unsigned int *totalpower, // array holding power per
						     // rx/tx channel
			   int firstPathDelay, // existing first path estimate
			   int *fineTimingRes, // result of algorithm
			   unsigned int *procBuffer, // buffer for processing,
						     // size 2*BUFFER_OFFSET DW
			   hal_wls_packet_params_t *packetparams // passing
								 // packetinfo
)
{
	int nRx = pktinfo->nRx + 1;
	int nTx = pktinfo->nTx + 1;
	int subsp_dim_L = SUB_DIM_L_20;
	float sub_det_thresh, sub_det_thresh_rel, music_thresh_rel;
	int fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW;
	float *realCorrMatrix =
		(float *)procBuffer; // realCorrMatrix[SUBSP_DIM_L*SUBSP_DIM_L]
	float *eigValPlusVec =
		(float *)procBuffer +
		BUFFER_OFFSET; // eigValPlusVec[MAX_MAT_SIZE+MAX_MAT_SIZE*MAX_MAT_SIZE]
	float *eMatrix =
		(float *)procBuffer; // eMatrix[2*SUBSP_DIM_L*SUBSP_DIM_L]
#ifdef USE_TLS
	float *ePeMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET; // ePeMatrix[4*(SIG_SUBSP_DIM_MAX-2)*(SIG_SUBSP_DIM_MAX-2)]
	float *eigValPlusVecEE = (float *)
		procBuffer; // eigValPlusVec[MAX_MAT_SIZE+MAX_MAT_SIZE*MAX_MAT_SIZE]
	float *vMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET; // vMatrix[4*(SUBSP_DIM_L-2)*(SUBSP_DIM_L-2)]
	float *qMatrix =
		(float *)procBuffer; // qMatrix[MAX_MAT_SIZE*MAX_MAT_SIZE]
	float *qPvMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET / 2; // qPvMatrix[SUBSP_DIM_L*SUBSP_DIM_L]
	float *psiHatMatrix =
		(float *)procBuffer; // psiHatMatrix[SUBSP_DIM_L*SUBSP_DIM_L]
#else
	float *qMatrix = (float *)procBuffer +
			 BUFFER_OFFSET; // qMatrix[MAX_MAT_SIZE*MAX_MAT_SIZE]
	float *qPeMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET * 3 / 2; // qPeMatrix[SUBSP_DIM_L*SUBSP_DIM_L]
	float *psiHatMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET; // psiHatMatrix[SUBSP_DIM_L*SUBSP_DIM_L]
#endif
	float *eigPsiHat = (float *)procBuffer +
			   BUFFER_OFFSET / 2; // eigPsiHat[SUBSP_DIM_L]
	float *omega_hat_sort =
		(float *)procBuffer; // omega_hat_sort[SUBSP_DIM_L]
	float *tau_hat =
		(float *)procBuffer + BUFFER_OFFSET / 4; // tau_hat[SUBSP_DIM_L]
	float *aMatrix =
		(float *)procBuffer +
		BUFFER_OFFSET / 2; // aMatrix[MAX_TX*MAX_RX*2*SUBSP_DIM_L]

	int numObs, sigSpaceDim, FD_downsample = 1;

	if ((pktinfo->packetType == 0) && (pktinfo->sigBw == 0) &&
	    (pktinfo->rxDevBw == 2)) { // process legacy pkt
		subsp_dim_L = SUB_DIM_L_6;
		sub_det_thresh = SUB_DET_THRESH_Legacy;
		sub_det_thresh_rel = SUB_DET_THRESH_REL_Legacy;
	} else if (packetparams->chNum >= 36) { // 5G band
		if (pktinfo->sigBw == 2) {
			sub_det_thresh = SUB_DET_THRESH_5G_80MHZ;
			sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_80MHZ;
		} else if (pktinfo->sigBw == 1) {
			sub_det_thresh = SUB_DET_THRESH_5G_40MHZ;
			sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_40MHZ;
		} else if (pktinfo->sigBw == 0) {
#if 0 // larger auto-correlation matrix size
			sub_det_thresh = SUB_DET_THRESH_5G_20MHZ;
			sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ;
#else
			fft_input_size = FFT_INPUT_SIZE_SHORT_WINDOW;
			subsp_dim_L = SUB_DIM_L_10;
			sub_det_thresh = SUB_DET_THRESH_5G_20MHZ;
			sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ;
#endif
		} else if (pktinfo->packetType < 2) {
			subsp_dim_L = SUB_DIM_L_16;
			fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW;
			sub_det_thresh = SUB_DET_THRESH_20MHZ;
			sub_det_thresh_rel = SUB_DET_THRESH_REL_5G_20MHZ;
		} else {
			FD_downsample = 4;
			subsp_dim_L = SUB_DIM_L_10;
			sub_det_thresh = SUB_DET_THRESH_Legacy;
			sub_det_thresh_rel = 0.035f;
		}
	} else { // 2.4G band
		if (pktinfo->sigBw == 1) {
			subsp_dim_L = SUB_DIM_L_16;
		} else {
			subsp_dim_L = SUB_DIM_L_12;
		}
		sub_det_thresh = SUB_DET_THRESH_20MHZ;
		sub_det_thresh_rel = SUB_DET_THRESH_REL_2G;
		fft_input_size = FFT_INPUT_SIZE_LONG_WINDOW;
	}

	// determine firstpath power threshold according to frequency, and pass
	// to "determineFirstPath"
	music_thresh_rel = (packetparams->chNum < 36) ? MUSIC_THRESH_REL_2GHz :
							MUSIC_THRESH_REL_5GHz;

	// in:	fftOutBuffer[], totalpower[]
	// out:	realCorrMatrix[SUBSP_DIM_L*SUBSP_DIM_L] {100 DW}
	calcCorMatrix(pktinfo, fftOutBuffer, totalpower, firstPathDelay,
		      realCorrMatrix, procBuffer + BUFFER_OFFSET, subsp_dim_L,
		      fft_input_size, FD_downsample, &numObs);
	// numObs = (fft_input_size - subsp_dim_L + 1) * 2 * nRx*nTx;
	sub_det_thresh *= (numObs * 2 * nRx * nTx); // account for CorMatrix
						    // scaling

	// eigen(Shur) decomposition
	// in:	realCorrMatrix[]
	// out:	eigValPlusVec[]; [D:MAX_MAT_SIZE (eigen values)|
	// Q:SUBSP_DIM_L*SUBSP_DIM_L (eigen vectors)] {10+100 DW +30DW scratch}
	QR_algorithm(realCorrMatrix, eigValPlusVec, subsp_dim_L, 0);

	// find signal space Eig Vals and construct E matrix for ESPRIT
	// in:	eigValPlusVec[] - used also as scratch memory
	// out:	eMatrix[]; [E_lower: (SUBSP_DIM_L-1)*SIG_SUBSP_DIM_MAX| E_upper:
	// (SUBSP_DIM_L-1)*SIG_SUBSP_DIM_MAX] {200 DW}
	sigSpaceDim = findSigSpace(eigValPlusVec, eMatrix, sub_det_thresh,
				   subsp_dim_L, pktinfo, sub_det_thresh_rel);

	if (sigSpaceDim < 2) {
		if (sigSpaceDim == 0) { // error, no signal over threshold
			return -1;
		}
		// else{ // (sigSpaceDim == 1)
		//	// for single eigen value, use simpler code
		//	// in: eMatrix[]
		//	// out: omega_hat_sort[]
		//	calcDimOnePhase(eMatrix, omega_hat_sort, SUBSP_DIM_L);

		//	// determine delays and amplitudes
		//	// in : fftOutBuffer[], totalpower[], omega_hat_sort[]
		//	// out: tau_hat[], aMatrix[]  {10+80 DW}
		//	calcDelayAmplitude(pktinfo, fftOutBuffer, totalpower,
		//firstPathDelay, sigSpaceDim, omega_hat_sort, tau_hat,
		//aMatrix);

		//	// determine first path
		//	// in: tau_hat[], aMatrix[]
		//	// out: fineTimingRes
		//	*fineTimingRes = determineFirstPath(pktinfo, tau_hat,
		//aMatrix, sigSpaceDim);

		//	return 0;
		//}
	}
#ifdef USE_TLS // QR of E
	// calc E'*E
	// in: eMatrix[]
	// out: ePeMatrix[] {256 DW}
	calcEtimesE(eMatrix, ePeMatrix, (subsp_dim_L - 1), 2 * sigSpaceDim);
	// eigen(Shur) decomposition
	// in:	ePeMatrix[]
	// out:	eigValPlusVec[]; [D:2*SIG_SUBSP_DIM_MAX (eigen values)|
	// Q:4*SIG_SUBSP_DIM_MAX^2 (eigen vectors)] {12+144 DW +30DW scratch}
	QR_algorithm(ePeMatrix, eigValPlusVecEE, 2 * sigSpaceDim, 1);

	// in: eigValPlusVec[]
	// out: vMatrix[]; [V_upper: SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX|
	// V_lower: SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX] {256 DW}
	constructVmatrix(eigValPlusVecEE, vMatrix, sigSpaceDim,
			 2 * sigSpaceDim);

	//  Psi_hat = V_lower\V_upper;
	// V_lower = Q*R
	// in: vMatrix[] V_lower (updated)
	// out: qMatrix[SIG_SUBSP_DIM_MAX*SIG_SUBSP_DIM_MAX] {100 DW }
	QR_decomposition(vMatrix + sigSpaceDim * SUBSP_DIM_L_MAX, qMatrix,
			 sigSpaceDim, sigSpaceDim);
	// calc Q'*V_upper
	// in: vMatrix[] V_upper, qMatrix[]
	// out: qPvMatrix[] {100 DW +30 DW scratch}
	calc_QpTimesE(vMatrix, qMatrix, qPvMatrix, sigSpaceDim, sigSpaceDim);
	// Psi_hat = (R^-1)(Q'*E_upper) solve with back substitution
	// in: qPvMatrix[], vMatrix[] V_lower (updated)
	// out: Psi_hat[]: (sigSpaceDim*sigSpaceDim) {100 DW}
	myBackSub(qPvMatrix, vMatrix + sigSpaceDim * SUBSP_DIM_L_MAX,
		  psiHatMatrix, sigSpaceDim, sigSpaceDim);

#else
	// LS Psi_hat = E_lower\E_upper;
	// E_lower = Q*R
	// in: eMatrix[] E_lower (updated)
	// out: qMatrix[] {100 DW}
	QR_decomposition(eMatrix, qMatrix, (subsp_dim_L - 1), sigSpaceDim);
	// calc Q'*E_upper
	// in: eMatrix[] E_upper, qMatrix[]
	// out: qPeMatrix[] {100 DW}
	calc_QpTimesE(eMatrix + SUBSP_DIM_L_MAX * sigSpaceDim, qMatrix,
		      qPeMatrix, (subsp_dim_L - 1), sigSpaceDim);
	// Psi_hat = (R^-1)(Q'*E_upper) solve with back substitution
	// in: qPeMatrix[], eMatrix[] E_lower (updated)
	// out: Psi_hat[]: (sigSpaceDim*sigSpaceDim) {100 DW}
	myBackSub(qPeMatrix, eMatrix, psiHatMatrix, (subsp_dim_L - 1),
		  sigSpaceDim);

#endif
	// omega_hat = atan(real(eig(Psi_hat)))
	// in: psiHatMatrix[]
	// out: eigPsiHat[]  {10 DW  +30 DW scratch}
	unsym_QR_algorithm(psiHatMatrix, eigPsiHat, sigSpaceDim);

	// determine phase
	// in: eigPsiHat[], sigSpaceDim
	// out: omega_hat_sort[], sigSpaceDim  {10 DW}
	sigSpaceDim = calcPhase(eigPsiHat, sigSpaceDim, omega_hat_sort);

	// determine delays and amplitudes
	// in : fftOutBuffer[], totalpower[], omega_hat_sort[]
	// out: tau_hat[], aMatrix[]  {10+80 DW}
	calcDelayAmplitude(pktinfo, fftOutBuffer, totalpower, firstPathDelay,
			   sigSpaceDim, omega_hat_sort, tau_hat, aMatrix,
			   fft_input_size);

	// determine first path
	// in: tau_hat[], aMatrix[]
	// out: fineTimingRes
	*fineTimingRes = determineFirstPath(pktinfo, tau_hat, aMatrix,
					    sigSpaceDim, music_thresh_rel);

	return 0;
}
