/*
 * Initialise variables and SI4432 for the low frequency sweep
 */
void initBandscope()
{
	ClearDisplay ();
/*
 *	Set up the "img" Sprite. This is the image for the graph. It makes for faster display
 *	updates and less flicker.
 *
 *	16 bit colour depth is faster than 8 and much faster than 4 bit! BUT - sprites
 *	pushed to it do not have correct colour - 8 bit and it is fine.
 *
 *	All marker sprites are WHITE for now.
 */
 	tft.unloadFont();
 	
 	img.unloadFont();
	img.deleteSprite();
	
	img.setTextSize ( 1 );
	img.setColorDepth ( 16 );
	img.setAttribute ( PSRAM_ENABLE, false );		// Don't use the PSRAM on the WROVERs
	img.createSprite ( 2, GRID_HEIGHT + 1 );    // Only 2 columns wide


/*
 *  The "tSprite" is used for displaying the data above the scan grid.
 */

	tSprite.deleteSprite();
	tSprite.setRotation ( 0 );
	tSprite.setTextSize ( 1 );
	tSprite.setColorDepth ( 16 );          
	tSprite.setAttribute ( PSRAM_ENABLE, false );		// Don't use the PSRAM on the WROVERs
	tSprite.createSprite ( tft.width() - X_ORIGIN, Y_ORIGIN );

	sSprite.deleteSprite();
/*
 *  Create and draw the sprite for the gain scale
 */
	CreateGainScale ();

	// Make sure everything will be reset
	old_settingAttenuate = -1000;
	old_settingPowerGrid = -1000;
	old_settingMax = -1;
	old_settingMin = -1;
	old_startFreq = -1;
	old_stopFreq = -1;
	old_ownrbw = -1;
	old_vbw = -1;
	old_settingAverage = -1;
	old_settingSpur = -100;
	old_bandwidth = 0;

	SetRX ( 0 );								// LO to transmit, RX to receive

	xmit.SetDrive ( setting.Drive );					// Set  transmitter power level
	rcvr.SetPreampGain ( setting.PreampGain );

	sweepStartDone = false;		// Make sure this initialize is only done once per sweep
	initSweep = true;
	
	tinySA_mode = BANDSCOPE;
	setting.Mode = tinySA_mode;
	Serial.println("before reset bandscope stack");
	ResetBandscopeMenuStack();			//  Put menu stack back to root level
	Serial.println("End of initBandscope"); 
}




/*
 *	This function section handles the fast bandscope sweep
 *	The display is split and shows a waterfall
 *	Number of points is reduced, and frequency change is done using an offset to aallow the delay time between
 *	changing frequency and taking a reading to be reduced
 */



void doBandscope()
{
static uint32_t		autoSweepStep = 0;
static uint32_t		autoSweepFreq = 0;
static uint32_t		autoSweepFreqStep = 0;
static uint32_t		nextPointFreq = 0;		// Frequency for the next display point. Used for substeps
static unsigned long	setFreqMicros;
static unsigned long	nowMicros;

static uint32_t		sweepStep;				// Step count
static uint32_t		sweepFreqStep;

static int16_t		pointMinGain;			// to record minimum gain for the current display point
static int16_t		pointMaxRSSI;			// to record max RSSI of the samples in the current display point
static uint32_t		pointMaxFreq;			// record frequency where maximum occurred

static int16_t		lastMode; 				// Record last operating mode (sig gen, normal)

static uint16_t		currentPointRSSI;
static uint16_t		peakRSSI;
static uint16_t		prevPointRSSI;
static uint32_t		peakFreq;
static uint16_t		peakIndex;
static uint16_t		pointsPastPeak;
static uint16_t		pointsPastDip;
static uint16_t		minRSSI;			// Minimum level for the sweep
static uint16_t		lastMinRSSI;		// Minimum level for the previous sweep

static bool			resetAverage;		// Flag to indicate a setting has changed and average valuesneeds to be reset

static bool			jsonDocInitialised = false;
static uint16_t		chunkIndex;



 	/*
 	 * If paused and at the start of a sweep then do nothing
 	 */
 	if (!sweepStartDone && paused)
 		return;


/*
 *	If the "sweepStartDone" flag is false or if the "initSweep" flag is true, we need
 *	to set things up for the sweep. 
 */


	if (( !sweepStartDone || initSweep || changedSetting ) )
	{
	 	if ( initSweep || changedSetting )		//  Something has changed, or a first start, so need to owrk out some basic things
	 	{
			Serial.println("InitBandscope or changedSetting");
			sweepPoints = setting.BandscopePoints;
			autoSweepFreqStep = ( setting.BandscopeSpan ) / sweepPoints;
	 		
			vbw = autoSweepFreqStep / 1000.0;		// Set the video resolution
			ownrbw = 2.6;	// and fix the resolution bandwidth to 2.6kHz
	
			bandwidth = rcvr.SetRBW ( ownrbw * 10.0, &delaytime );	// Set it in the receiver Si4432

			//Serial.printf("set rcvr Freq get:%u, tempIF:%u\n", rcvr.GetFrequency(), tempIF);
			rcvr.SetFrequency ( setting.IF_Freq );		// Set the RX Si4432 to the IF frequency
	

			sweepFreqStep = autoSweepFreqStep;	// Step for each reading

			if ( setting.Attenuate != old_settingAttenuate )
			{
				if ( !att.SetAtten ( setting.Attenuate ))		// Set the internal attenuator
					setting.Attenuate = att.GetAtten ();		// Read back if limited (setting.Attenuate was outside range)
					old_settingAttenuate = setting.Attenuate;
			}

			resetAverage = changedSetting;


#ifdef USE_WIFI
			//  Vary number of points to send in each chunk depending on delaytime
			//  A chunk is sent at the end of each sweep regardless
			wiFiPoints = wiFiTargetTime / delaytime;
			if (wiFiPoints > MAX_WIFI_POINTS)
				wiFiPoints = MAX_WIFI_POINTS;
			if (wiFiPoints > setting.BandscopePoints)
				wiFiPoints = setting.BandscopePoints;
			Serial.printf("No of wifiPoints set to %i\n", wiFiPoints);
							
			if ( numberOfWebsocketClients > 0 )
				pushBandscopeSettings (); 
#endif    									// #ifdef USE_WIFI



	 	}	// initSweep || changedSetting

		autoSweepStep = 0;						// Set the step counter to zero
		autoSweepFreq = setting.BandscopeStart;		// Set the start frequency.

		nextPointFreq = autoSweepFreq + autoSweepFreqStep;


		while (( micros() - setFreqMicros ) < delaytime )  	// Make sure enough time has elasped since previous frequency write
			{
			}

		setFreqMicros = micros();						// Store the time the frequency was changed
		xmit.SetFrequency ( setting.IF_Freq + autoSweepFreq );	  // set the LO frequency, tempIF is offset if spur reduction on


#ifdef USE_WIFI

		if ( numberOfWebsocketClients > 0 )	//  Start off the json document for the scan
		{
			jsonDocument.clear ();
			chunkIndex = 0;

			jsonDocument["PreAmp"] = setting.PreampGain;
			jsonDocument["mType"] = "chunkSweep";
			jsonDocument["StartIndex"] = 0;
			jsonDocument["sweepPoints"] = sweepPoints;
			jsonDocument["sweepTime"] = (uint32_t)(sweepMicros/1000);
			Points = jsonDocument.createNestedArray ( "Points" );	// Add Points array
			jsonDocInitialised = true;
		}

		else
			jsonDocInitialised = false;

#endif										// #ifdef USE_WIFI

		sweepStep = 0;
		startFreq   = setting.BandscopeStart + setting.IF_Freq;		// Start freq for the LO
		stopFreq    = setting.BandscopeSpan  + startFreq;		// Stop freq for the LO

		Serial.printf(" start %i; stop %i; points %i \n", startFreq, stopFreq, sweepPoints );

		if ( setActualPowerRequested )
		{
			SetPowerLevel ( actualPower );
			setActualPowerRequested = false; 

//			Serial.printf ( "Setting actual Power %f \n", actualPower );
		}

		pointMinGain = 100;						// Reset min/max values
		pointMaxRSSI = 0;


/*
*	Copy the values for the peaks (marker positions) to the old versions. No need to
*	reset the indicies or frequencies; just the "Level".
*/
		for ( int i = 0; i < MARKER_COUNT; i++ )
		{
			oldPeaks[i].Level = peaks[i].Level;
			oldPeaks[i].Index = peaks[i].Index;
			oldPeaks[i].Freq  = peaks[i].Freq;
			peaks[i].Level    = 0;
		}

		//DisplayInfo ();						// Display axis, top and side bar text

		peakLevel = 0;						// Reset the peak values for the sweep
		peakFreq = 0.0;
		peakGain = 100;						// Set to higher than gain can ever be
    
		lastMinRSSI = minRSSI;
		minRSSI = 300;						// Higher than it can be



		pointsPastPeak = 0;			// Avoid possible peak detection at start of sweep
		peakRSSI = 0;

		sweepStartDone = true;		// Make sure this initialize is only done once per sweep
		initSweep = false;
		changedSetting = false;

		lastSweepStartMicros = sweepStartMicros;		// Set last time we got here
		sweepStartMicros = micros();					// Current time
		sweepMicros = sweepStartMicros - lastSweepStartMicros; // Calculate sweep time (no rollover handling)

	}									// End of "if ( !sweepStartDone ) || initSweep || changedSetting )"


/*
*	Here we do the actual sweep. Save the current step and frequencies for the next time
*	through, then wait the required amount of time based on the RBW before taking the
*	signal strength reading and changing the transmitter (LO) frequency.
*/

	uint16_t	oldSweepStep = autoSweepStep;
	uint32_t	oldSweepFreq = autoSweepFreq;

	/*
	 *	Wait until time to take the next reading. If a long wait then check the touchscreen
	 *	and Websockets while we are waiting to improve response
	 */
	nowMicros = micros();

	while (( nowMicros - setFreqMicros ) < delaytime )
	{

		if ( ( nowMicros - setFreqMicros + delaytime > 200 ) &&
			( (nowMicros - lastWebsocketMicros > websocketInterval) || (numberOfWebsocketClients > 0) ) )
		{
//			Serial.print("W");
	    	webSocket.loop ();					// Check websockets - includes Yield() to allow other events to run
//			Serial.println("w");
			lastWebsocketMicros = nowMicros;
		}
		if ( nowMicros - setFreqMicros > 100 )		// Wait some time to allow DMA sprite write to finish!
				UiProcessTouch ();						// Check the touch screen
//		Serial.println("w");
		nowMicros = micros();
	}

	int rxRSSI = rcvr.GetRSSI ();						// Read the RSSI from the RX SI4432

/*
 *	Note that there are two different versions of the print statement to send the 
 *	RSSI readings to the serial output. You can change which one is commented out.
 *
 *	The first one produces a tab separated list of just the frequency and RSSI
 *	reading. That format can be easily read inte something like Excel.
 *
 *	The second one produces a listing more fit for human consumption!
 */

//	if ( showRSSI )												// Displaying RSSI?
//	{
//		Serial.printf ( "%s\t%03d\n",
//					FormatFrequency ( autoSweepFreq) , rxRSSI );	// Send it to the serial output
		Serial.printf ( "Freq: %s - RSSI: %03d\n",
					FormatFrequency ( autoSweepFreq) , rxRSSI );	// Send it to the serial output
//	}

	if ( (numberOfWebsocketClients > 0) || (setting.ShowGain) )
		gainReading = GetPreampGain ( &AGC_On, &AGC_Reg );		// Record the preamp/lna gains

	autoSweepFreq += sweepFreqStep;			// Increment the frequency
	sweepStep++;							// and increment the step count


/*
 *	Change the transmitter frequency for the next reading and record the time for
 *	the RBW required settling delay.
 */
 	uint32_t f = setting.IF_Freq + autoSweepFreq;

	setFreqMicros = micros();					// Store the time the LO frequency was changed
	xmit.SetFrequency ( f ); // Set the new LO frequency as soon as RSSI read


#ifdef USE_WIFI

	if ( numberOfWebsocketClients > 0 ) 
	{
		if ( jsonDocInitialised )
		{
			JsonObject dataPoint = Points.createNestedObject ();	// Add an object to the Json array to be pushed to the client
			dataPoint["x"] = oldSweepFreq/1000000.0;				// Set the x(frequency) value
			dataPoint["y"] = rxRSSI; 								// Set the y (RSSI) value
	        chunkIndex++;					// increment no of data points in current WiFi  chunk
	
			if ( chunkIndex >= wiFiPoints )		// Send the chunk of data and start new jSon document
			{
				String wsBuffer;
	
				if ( wsBuffer )
				{
	//				Serial.print("D");
					serializeJson ( jsonDocument, wsBuffer );
	//				Serial.printf("J%u", wsBuffer.length() );
					unsigned long s = millis();
					webSocket.broadcastTXT ( wsBuffer );		//  Send to all connected websocket clients
					if (millis() - s > 1000)
					{
						Serial.println("webSocketTimeout");
						Serial.println(wsBuffer);
						numberOfWebsocketClients = 0;
					}
	//				Serial.print("j");
				}
	
				else
					Serial.println("No buffer :(");
			}
		}
		if ( ( chunkIndex >= wiFiPoints ) || !jsonDocInitialised )		// Start new jSon document
		{
			chunkIndex = 0;
			jsonDocument.clear();
			jsonDocument["mType"] = "chunkSweep";
			jsonDocument["StartIndex"] = sweepStep;
			jsonDocument["sweepPoints"] = sweepPoints;
			jsonDocument["sweepTime"] = (uint32_t)(sweepMicros/1000);
			Points = jsonDocument.createNestedArray  ("Points" );	// Add Points array
			jsonDocInitialised = true;
		}
	}

#endif											// #ifdef USE_WIFI

	myActual[autoSweepStep] = rxRSSI;		

	myGain[autoSweepStep] = gainReading;	

	DrawCheckerBoard ( oldSweepStep );					// Draw the grid for the point in the sweep we have just read

		if ( resetAverage || setting.Average == AV_OFF )	// Store data, either as read or as rolling average
			myData[oldSweepStep] = myActual[oldSweepStep];

		else
		{
			switch ( setting.Average )
			{
				case AV_MIN:
					if ( myData[oldSweepStep] > myActual[oldSweepStep] )
						myData[oldSweepStep] = myActual[oldSweepStep];
					break;

				case AV_MAX:
					if ( myData[oldSweepStep] < myActual[oldSweepStep] )
						myData[oldSweepStep] = myActual[oldSweepStep];
					break;

				case AV_2:
					myData[oldSweepStep] = ( myData[oldSweepStep] + myActual[oldSweepStep] ) / 2;
					break;

				case AV_4:
					myData[oldSweepStep] = ( myData[oldSweepStep]*3 + myActual[oldSweepStep] ) / 4;
					 break;

				case AV_8:
					myData[oldSweepStep] = ( myData[oldSweepStep]*7 + myActual[oldSweepStep] ) / 8;
					break;
			}
			DisplayPoint ( myData, oldSweepStep, AVG_COLOR );
		}

		if ( setting.ShowSweep )
			DisplayPoint ( myActual, oldSweepStep, DB_COLOR );

		if ( setting.ShowGain )
			displayGainPoint ( myGain, oldSweepStep, GAIN_COLOR );

		if ( setting.ShowStorage )
			DisplayPoint ( myStorage, oldSweepStep, STORAGE_COLOR );

		if ( setting.SubtractStorage ) 
			rxRSSI = 128 + rxRSSI - myStorage[oldSweepStep];


/*
 *	Record the peak values but not if freq low enough to detect the LO
 */

		if ( peakLevel < myData[oldSweepStep] )
		{
			peakIndex = oldSweepStep;
			peakLevel = myData[oldSweepStep];
			peakFreq  = oldSweepFreq;

//				Serial.printf( "peakLevel set %i, index %i\n", peakLevel, oldSweepStep);
//				displayPeakData ();
		}


/*       
 *	Save values used by peak detection. Need to save the previous value as we only
 *	know we have a peak once past it!
 */

			prevPointRSSI = currentPointRSSI;
			currentPointRSSI = myData[oldSweepStep];


/*       
 *     Peak point detection. Four peaks, used to position the markers
 */
			if ( currentPointRSSI >= prevPointRSSI )		// Level or ascending
			{
				pointsPastDip ++;
				if ( pointsPastDip == PAST_PEAK_LIMIT )
				{ 
					pointsPastPeak = 0;
				}

				if ( currentPointRSSI > peakRSSI )
				{
					peakRSSI = currentPointRSSI;			// Store values
					peakFreq = oldSweepFreq;
					peakIndex = oldSweepStep;
				}
			}

			else
			{
				pointsPastPeak ++;				// only a true peak if value decreased for a number of consecutive points 

				if ( pointsPastPeak == PAST_PEAK_LIMIT )	// We have a peak
				{
					pointsPastDip = 0;

/* 
 *	Is this peak bigger than previous ones? Only check if bigger than smallest peak so far
 */

					if ( peakRSSI > peaks[MARKER_COUNT-1].Level )
					{
						for ( uint16_t p = 0; p < MARKER_COUNT; p++ )
						{
							if ( peakRSSI > peaks[p].Level )
							{
								for ( uint16_t n = 3; n > p; n-- )	// Shuffle lower level peaks down
									memcpy ( &peaks[n], &peaks[n-1], sizeof ( peak_t ));

								peaks[p].Level = peakRSSI;			// Save the peak values
								peaks[p].Freq  = peakFreq;
								peaks[p].Index = peakIndex;
								break;
							}
						}
					}
         
					peakRSSI = 0;	// Reset peak values ready for next peak
				}					// We have a peak
			}						// Descending


/*
 *	Draw the markers if main sweep is displayed. The markers know if they are enabled or not
 *	Only paint if sweep step is in range where there will be a marker
 */

		if ( setting.ShowSweep || setting.Average != AV_OFF )
		{
			for ( int p = 0; p < MARKER_COUNT; p++ )
				if (( abs ( oldSweepStep - oldPeaks[p].Index ) 
						<= MARKER_SPRITE_HEIGHT / 2 ) && ( oldPeaks[p].Level > (lastMinRSSI + MARKER_NOISE_LIMIT) ))

					marker[p].Paint ( &img,  oldPeaks[p].Index - oldSweepStep, 
							rssiToImgY ( oldPeaks[p].Level ) );   
		}

		// If in the last few points and gain trace is displayed show the gain scale
		if ( setting.ShowGain && (oldSweepStep > setting.BandscopePoints - 2 * CHAR_WIDTH) )
		{
			int16_t scaleX = setting.BandscopePoints - 2 * CHAR_WIDTH - oldSweepStep + 1; // relative to the img sprite
			img.setPivot( scaleX, 0);
			gainScaleSprite.pushRotated ( &img, 0, TFT_BLACK );				// Send the sprite to the target sprite, with transparent colour
		}

		if ( oldSweepStep > 0 )				// Only push if not first point (two pixel wide img)
			img.pushSprite ( X_ORIGIN+oldSweepStep-1, Y_ORIGIN );

		myFreq[oldSweepStep] = oldSweepFreq;		// Store the frequency for XML file creation


	if ( sweepStep >= sweepPoints )		// If we have got to the end of the sweep
	{
//		autoSweepStep  = 0;
		sweepStartDone = false;
		resetAverage = false;

		if ( sweepCount < 2 )
			sweepCount++;				// Used to disable wifi at start

		oldPeakLevel = peakLevel;		//Save value of peak level for use by the "SetPowerLevel" function

		if ( myActual[setting.BandscopePoints-1] == 0 )		// Ensure a value in last data point
		{
			myActual[setting.BandscopePoints-1] = rxRSSI;	// Yes, save it
			myGain[setting.BandscopePoints-1] = gainReading;
			myFreq[setting.BandscopePoints-1] = oldSweepFreq;
		}

		if ( showRSSI == 1 )						// Only show it once?
			showRSSI = 0;							// Then turn it off

#ifdef USE_WIFI

		if (( numberOfWebsocketClients > 0) && jsonDocInitialised && (chunkIndex > 0) )
		{
			String wsBuffer;

			if (wsBuffer)
			{
				serializeJson ( jsonDocument, wsBuffer );
				webSocket.broadcastTXT ( wsBuffer );		// Send to all connected websocket clients
			}

			else
				Serial.println ( "No buffer :(");
		}

#endif							// #ifdef USE_WIFI   

	}							// End of "if ( sweepStep >= sweepPoints )"
}								// End of "doSweepLow"