/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

#include <arduinoFFT.h>
#include <SX1278.h>
#include <correction_values.h>


/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

#define VERBOSE	// print debug messages via USART
#define VERY_VERBOSE
#define I2C_RX 0
#define I2C_TX 1
#define DEVICE_COUNT 5

// FFT things
#define START_BIN 3
#define FFT_BINS 512	// Actually usable bins after FFT
#define SAMPLE_SIZE (FFT_BINS * 2)
#define BUFSIZE (SAMPLE_SIZE * 4)

// Audio stuff
#define MIC_OFFSET_DB 2
#define MIC_REF_DB 94.0
#define MIC_SENSITIVITY -26
#define MIC_SNR 65
#define MIC_BITS 16
#define MIC_OVERLOAD 120
#define MIC_NOISE_FLOOR (MIC_REF_DB - MIC_SNR)

// Power stuff
#define DROPOUT_VOLTAGE (3.3 + 0.25) 					// For a load of approx. 50 mA and LP2950ACZ-3.3G
#define BAT_VOLTAGE_CHECK_INTERVAL 60000	// ms
#define BAT_VOLTAGE_CHECK_TOLERANCE 500		// ms
#define BAT_FIELD_SIZE 1
#define BAT_LOW_THRESHOLD DROPOUT_VOLTAGE - 0.4		// This seems odd but below the drop out voltage, MCU and LoRa will still work
#define BAT_MEASUREMENT_CYCLES 10					// Measure over 10 iterations to avoid one-offs.

// LoRa stuff
#define LORA_SYNC_WORD 0x2AF69A00
#define LORA_TIMEOUT 200
#define LORA_HEADER_LENGTH sizeof(LORA_SYNC_WORD) / 2	// divide by two because we use a uint16_t (2 byte) buffer
														// just fine (min. 1.8V) so we can go a bit lower for longer battery life.
#define LORA_BEACON_SIZE 10


/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
DMA_HandleTypeDef hdma_adc1;

I2C_HandleTypeDef hi2c2;

I2S_HandleTypeDef hi2s1;
DMA_HandleTypeDef hdma_spi1_rx;

SPI_HandleTypeDef hspi2;

TIM_HandleTypeDef htim2;

UART_HandleTypeDef huart4;

/* USER CODE BEGIN PV */
// TODO CHECK
constexpr double MIC_REF_AMPL = pow(10, double(MIC_SENSITIVITY)/20) *  (pow(2, (MIC_BITS - 1)) - 1);

char msg[64];
uint8_t ret;
uint16_t i2s_dma_buf[BUFSIZE];
uint32_t adc_buf[3];
double vReal[SAMPLE_SIZE];
double vImag[SAMPLE_SIZE];

volatile bool adc_ready;

volatile uint32_t adc_avg;
volatile float bat_voltage_avg;

bool got_i2c_setup_message;
volatile bool got_beacon;
volatile bool flip_buffer;
volatile bool tx_due;
volatile uint8_t adc_counter;
volatile uint8_t tick_count = LORA_HEADER_LENGTH;
uint8_t verbosity;

uint8_t i2c_tx_counter;
uint16_t lora_beacon_rx_buffer[5];
uint16_t i2c_tx_buffer[1024];
uint16_t first_sync_message[5];

// Capitalized means "const" but we cannot make them real const as that doesn't work with the protcol.
uint8_t PACKET_LENGTH;
uint8_t LORA_PACKET_SIZE;
uint16_t TX_OFFSET;
uint16_t TICK_COUNT;
uint16_t measured_values1[63];	// TODO
uint16_t measured_values2[63];
uint32_t TICK_DURATION_US;


volatile uint8_t device_id;
char i2c_rx_buffer[4];


/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_ADC1_Init(void);
static void MX_I2S1_Init(void);
static void MX_SPI2_Init(void);
static void MX_I2C2_Init(void);
static void MX_USART4_UART_Init(void);
static void MX_TIM2_Init(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
SX1278_hw_t SX1278_hw;
SX1278_t SX1278;


void PrintVector(double *vData, uint16_t bufferSize, uint8_t scaleType) {
  for (uint16_t i = 0; i < bufferSize; i++) // First two bins are not meaningful.
  {
    char buf[10];
    char buf2[bufferSize];
    double abscissa;
    /* Print abscissa value */
    switch (scaleType)
    {
      case 0:
        abscissa = (i * 1.0);
	break;
      case 1:
        abscissa = ((i * 1.0) / hi2s1.Init.AudioFreq);
	break;
      case 2:
        abscissa = ((i * 1.0 * hi2s1.Init.AudioFreq) / SAMPLE_SIZE);
	break;

      default:
    	  break;
    }

    sprintf(buf, "%.4f", abscissa);
    HAL_UART_Transmit(&huart4, (uint8_t *) buf, strlen(buf), 100);

    if(scaleType==2) {
    	HAL_UART_Transmit(&huart4, (uint8_t *) " Hz", 3, 100);
    }

    HAL_UART_Transmit(&huart4, (uint8_t *) " ", 1, 100);

    sprintf(buf2, "%.4f, %.4f dB\n", vData[i], (20 * log10(vData[i])));
    HAL_UART_Transmit(&huart4, (uint8_t *) buf2, strlen(buf2), 100);
  }
  HAL_UART_Transmit(&huart4, (uint8_t *) "\n", 1, 100);
}


/**
 * Helper function to split the LoRa sync word from the device ID in a LoRa message.
 */

_Bool IsOwnLoraPacket(uint16_t *buf) {
	if (buf[0] == (LORA_SYNC_WORD >> 16) && (buf[1] & 0xff00) == (uint16_t) (LORA_SYNC_WORD)   ) {
		return 1;
	}

	return 0;
}

uint8_t GetDeviceId(uint16_t *buf) {
	return (buf[1] & 0x00ff);
}


/*
 * Do FFT on the filled arrays, calculate RMS and return the A-weighted dB value.
 */
uint16_t GetDBA(double *vReal, double *vImag) {
	arduinoFFT fft = arduinoFFT(vReal, vImag, SAMPLE_SIZE, hi2s1.Init.AudioFreq);
	fft.Windowing(FFT_WIN_TYP_HANN, FFT_FORWARD);
	fft.Compute(FFT_FORWARD);
	fft.ComplexToMagnitude();

	double squared_sum = 0;
	for (uint16_t i = START_BIN; i < FFT_BINS; i++) {
		squared_sum += (pow(vReal[i], 2) * CORRECTION_VALUES[i]);	// Parseval's Theorem
	}

	double rms = 1.63 * sqrt(2.0 * squared_sum / (SAMPLE_SIZE * SAMPLE_SIZE));	// https://de.mathworks.com/matlabcentral/answers/372516-calculate-windowing-correction-factor
	double dbA = MIC_OFFSET_DB + MIC_REF_DB + 20 * log10(rms / MIC_REF_AMPL);

	// We can't measure below that as the noise from the microphone is louder than the ambient sound itself.
	if (dbA <= MIC_NOISE_FLOOR) {
		dbA = MIC_NOISE_FLOOR;
	} else if (dbA >= MIC_OVERLOAD) {
		dbA = MIC_OVERLOAD;
	}

	return (dbA * 10); 	// Make a fixed point from the double. Give us one decimal point of precision which is plenty.
}


/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC1_Init();
  MX_I2S1_Init();
  MX_SPI2_Init();
  MX_I2C2_Init();
  MX_USART4_UART_Init();
  MX_TIM2_Init();
  /* USER CODE BEGIN 2 */

#ifdef VERBOSE
	verbosity = 1;
#endif

#ifdef VERY_VERBOSE
	verbosity = 2;
#endif

  //SX1278_hw.dio0.port = DIO0_GPIO_Port;
  //SX1278_hw.dio0.pin = DIO0_;
  SX1278_hw.dio0.port = GPIOB;
  SX1278_hw.dio0.pin = GPIO_PIN_0;
  SX1278_hw.nss.port = NSS_GPIO_Port;
  SX1278_hw.nss.pin = NSS_Pin;
  SX1278_hw.reset.port = RESET_GPIO_Port;
  SX1278_hw.reset.pin = RESET_Pin;
  SX1278_hw.spi = &hspi2;
  SX1278.hw = &SX1278_hw;

  SX1278_init(&SX1278, 866000000, SX1278_POWER_14DBM, SX1278_LORA_SF_8, SX1278_LORA_BW_250KHZ, SX1278_LORA_CR_4_5, SX1278_LORA_CRC_EN, 60);

  HAL_I2S_Receive_DMA(&hi2s1, (uint16_t *) i2s_dma_buf, BUFSIZE/2);

  // This would work just fine without DMA but multiple channels don't work in polling mode.
  HAL_TIM_Base_Start(&htim2);
  HAL_ADC_Start_DMA(&hadc1, adc_buf, hadc1.Init.NbrOfConversion);
  while (!adc_ready);
  adc_ready = 0;
  HAL_ADC_Stop_DMA(&hadc1);
  uint32_t adc_value = adc_buf[0];

  if (verbosity >= 2) {
	  sprintf(msg, "%lu %lu\n", adc_buf[0], adc_buf[1]);
	  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
  }

	if (adc_value <= 400) { // LOW
		device_id = 4;
	} else if (adc_value >= 600 && adc_value <= 1400) { // 3V3 -- 10k -- 3k3 -- GND
		device_id = 1;
	} else if (adc_value >= 1600 && adc_value <= 2400) { // 3V3 -- 10k -- 10k -- GND
		device_id = 2;
	} else if (adc_value >= 2600 && adc_value <= 3400) { // 3V3 -- 10k -- 33k -- GND
		device_id = 3;
	} else if (adc_value >= 3600) { // HIGH
		device_id = 0; // Master
		HAL_I2C_EnableListen_IT(&hi2c2);
	}

	if (device_id == 0) {
		while (!got_i2c_setup_message) { HAL_UART_Transmit(&huart4, (uint8_t *) "wait for i2c\n", 13, 100); }
		got_i2c_setup_message = 0;

		TICK_COUNT = i2c_rx_buffer[1];
		TICK_DURATION_US = i2c_rx_buffer[2] * 1000000;	// Seconds to µs.
		TX_OFFSET = i2c_rx_buffer[3];	// In ticks
		PACKET_LENGTH = (TICK_COUNT + LORA_HEADER_LENGTH + BAT_FIELD_SIZE);
		LORA_PACKET_SIZE = PACKET_LENGTH * 2;


		first_sync_message[0] = (uint16_t) (LORA_SYNC_WORD >> 16);
		first_sync_message[1] = (uint16_t) (LORA_SYNC_WORD | device_id);
		first_sync_message[2] = TICK_COUNT;	// Tick count
		first_sync_message[3] = i2c_rx_buffer[2];	// Tick duration
		first_sync_message[4] = TX_OFFSET;	// Tx offset

		if (verbosity >= 1) {
			sprintf(msg, "tc: %u, td: %u, tx_offs: %u\n", first_sync_message[2], i2c_rx_buffer[2], first_sync_message[4]);
			HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
		}

	}

	uint16_t lora_rx_buffer[PACKET_LENGTH];
	if (verbosity >= 1) {
		sprintf(msg, "My device ID is %u\n", device_id);
		HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
	}


  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

	  // Master
	  if (device_id == 0) {

		  __HAL_TIM_SET_COUNTER(&htim2, 0);


		  // Send a sync beacon at the start of every "period".
		  if (tick_count == 2) {
			  SX1278_LoRaEntryTx(&SX1278, LORA_BEACON_SIZE, LORA_TIMEOUT);

			  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);

			  SX1278_LoRaTxPacket(&SX1278, (uint8_t *) &first_sync_message, LORA_BEACON_SIZE, LORA_TIMEOUT);
			  //SX1278_LoRaTxPacket(&SX1278, (uint8_t *) &measured_values1, 4, LORA_TIMEOUT);
			  HAL_UART_Transmit(&huart4, (uint8_t *) first_sync_message, LORA_BEACON_SIZE, 100);

			  if (verbosity >= 1) {
				  HAL_UART_Transmit(&huart4, (uint8_t *) "\nsent beacon\n", 13, 100);
			  } else if (verbosity >= 2) {
				  HAL_UART_Transmit(&huart4, (uint8_t *) first_sync_message, 10, 100);
			  }
			  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);

			  // Change back to Rx again.
			  SX1278_LoRaEntryRx(&SX1278, LORA_PACKET_SIZE, LORA_TIMEOUT);
		  }

		  for (volatile uint16_t i = 0; i < BUFSIZE; i += 4) {
			  vReal[i/4] = abs(65535 - i2s_dma_buf[i]);
			  vImag[i/4] = 0;
		  }

		  // For the master, one buffer is enough. No need to flip.
		  measured_values1[tick_count] = GetDBA(vReal, vImag);
		  if (verbosity >= 2) {
			  sprintf(msg, "fill array %u\n", tick_count);
			  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
		  }

		  //sprintf(msg, "array[%u]: %u\n", tick_count, i2c_tx_buffer[tick_count]);
		  //HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);

		  tick_count++;

		  if (tick_count == PACKET_LENGTH - BAT_FIELD_SIZE) {	// Last measurement, now wait for I²C
			  bool debug_sent = 0;
			  while (!got_i2c_setup_message) {
				  if (verbosity >= 1 && !debug_sent) {
					  HAL_UART_Transmit(&huart4, (uint8_t *) "Waiting for I2C\n", 16, 100);
					  debug_sent = 1;
				  }
			  }

			  if (verbosity >= 1) {
				  HAL_UART_Transmit(&huart4, (uint8_t *) "Got I2C sync\n", 13, 100);
			  }

			  memcpy(&i2c_tx_buffer[0], &measured_values1, LORA_PACKET_SIZE);

			  TICK_COUNT = i2c_rx_buffer[1];
			  TICK_DURATION_US = i2c_rx_buffer[2] * 1000000;	// Seconds to µs.
			  TX_OFFSET = i2c_rx_buffer[3];	// In ticks
			  PACKET_LENGTH = (TICK_COUNT + LORA_HEADER_LENGTH + BAT_FIELD_SIZE);
			  LORA_PACKET_SIZE = PACKET_LENGTH * 2;

			  first_sync_message[0] = (uint16_t) (LORA_SYNC_WORD >> 16);
			  first_sync_message[1] = (uint16_t) (LORA_SYNC_WORD | device_id);
			  first_sync_message[2] = TICK_COUNT;	// Tick count
			  first_sync_message[3] = i2c_rx_buffer[2];	// Tick duration
			  first_sync_message[4] = TX_OFFSET;	// Tx offset

			  got_i2c_setup_message = 0;
			  tick_count = LORA_HEADER_LENGTH;
			  sprintf(msg, "tickcountreset %u, gotmsg %u\n", tick_count, got_i2c_setup_message);
			  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);

		  } else {
			  while (__HAL_TIM_GET_COUNTER(&htim2) < TICK_DURATION_US) {
				  ret = SX1278_LoRaRxPacket(&SX1278);

				  // Only accept packages within our expected length to avoid buffer overflows.
				  if (ret > 0 && ret <= LORA_PACKET_SIZE) {
					  memset(lora_rx_buffer, 0, sizeof lora_rx_buffer);
					  SX1278_read(&SX1278, (uint8_t*) lora_rx_buffer, LORA_PACKET_SIZE);

					  if (IsOwnLoraPacket(lora_rx_buffer)) {
						  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);

						  uint8_t devid = GetDeviceId(lora_rx_buffer);
						  memcpy(&i2c_tx_buffer[devid * PACKET_LENGTH], &lora_rx_buffer, LORA_PACKET_SIZE);

						  if (verbosity >= 1) {
							  sprintf(msg, "Rx %u bytes: ", sizeof(lora_rx_buffer));
							  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
							  HAL_UART_Transmit(&huart4, (uint8_t *) lora_rx_buffer, LORA_PACKET_SIZE, 100);
							  HAL_UART_Transmit(&huart4, (uint8_t *) "\n", 1, 100);
						  }

						  HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
						  break;	// No need to check again as no two Rx can happen within one tick.
					  }
				  }
			  }
		  }

		  while (__HAL_TIM_GET_COUNTER(&htim2) < TICK_DURATION_US) {
			  // Just wait.
		  }

	  } else {
		  if (!got_beacon) {
			  SX1278_LoRaEntryRx(&SX1278, 4, LORA_TIMEOUT);
		  }

		  while (!got_beacon) {
			  ret = SX1278_LoRaRxPacket(&SX1278);
			  if (verbosity >= 2) { HAL_UART_Transmit(&huart4, (uint8_t *) "awaiting beacon\n", 16, 100); }

			  if (ret > 0 && ret <= LORA_BEACON_SIZE) {
				  HAL_UART_Transmit(&huart4, (uint8_t *) lora_beacon_rx_buffer, LORA_BEACON_SIZE, 100);
				  SX1278_read(&SX1278, (uint8_t*) lora_beacon_rx_buffer, LORA_BEACON_SIZE);

				  // Beacons only come from the master (device_id == 0)
				  if (IsOwnLoraPacket(lora_beacon_rx_buffer) && GetDeviceId(lora_beacon_rx_buffer) == 0) {
					  TICK_COUNT = lora_beacon_rx_buffer[2];
					  TICK_DURATION_US = lora_beacon_rx_buffer[3] * 1000000;
					  TX_OFFSET = lora_beacon_rx_buffer[4];
					  PACKET_LENGTH = (TICK_COUNT + LORA_HEADER_LENGTH + BAT_FIELD_SIZE);
					  LORA_PACKET_SIZE = PACKET_LENGTH * 2;
					  got_beacon = 1;

					  if (verbosity >= 2) {
						  sprintf(msg, "tc: %u, td: %lu, offs: %u\n", TICK_COUNT, TICK_DURATION_US, TX_OFFSET);
						  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
						  HAL_UART_Transmit(&huart4, (uint8_t *) lora_beacon_rx_buffer, LORA_BEACON_SIZE, 100);
						  HAL_UART_Transmit(&huart4, (uint8_t *) "\n", 1, 100);

					  }
					  break;
				  }
			  }
			  HAL_Delay(150);	// Going at full throttle just pisses away power.
		  }

		  sprintf(msg, "tc: %u, td: %lu, offs: %u\n", TICK_COUNT, TICK_DURATION_US, TX_OFFSET);
		  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
		  // Use the first 4 bytes as LoRa sync word and device ID.
		  measured_values1[0] = (uint16_t) (LORA_SYNC_WORD >> 16);
		  measured_values2[0] = (uint16_t) (LORA_SYNC_WORD >> 16);
		  measured_values1[1] = (uint16_t) (LORA_SYNC_WORD | device_id);
		  measured_values2[1] = (uint16_t) (LORA_SYNC_WORD | device_id);

		  // Battery state.
		  measured_values1[PACKET_LENGTH - 1] = 0x0000;
		  measured_values2[PACKET_LENGTH - 1] = 0x0000;


		  __HAL_TIM_SET_COUNTER(&htim2, 0);

		  // TICK_COUNT / 2 avoids measurement during Tx which drops the voltage a fair bit.
		  if (device_id != 0 && tick_count == (TICK_COUNT / 2)) {
			  HAL_ADC_Start_DMA(&hadc1, adc_buf, hadc1.Init.NbrOfConversion);
			  while (!adc_ready);
			  adc_ready = 0;
			  HAL_ADC_Stop_DMA(&hadc1);
			  adc_avg += adc_buf[1];
			  adc_counter++;

			  if (adc_counter == BAT_MEASUREMENT_CYCLES) {
				  HAL_ADCEx_Calibration_Start(&hadc1);		// As per data sheet this needs to happen while the ADC is not running.
				  uint32_t cal = HAL_ADCEx_Calibration_GetValue(&hadc1) / 2;	// Returns an offset we add to the measurement later on.

				  adc_avg /= adc_counter;
				  bat_voltage_avg = ((adc_avg + cal) * 1.212 / adc_buf[2]) * 2;		// 1.212 is the VREFINT voltage as per data sheet.

				  if (verbosity >= 2) {
					  sprintf(msg, "V_BAT: %.4f, ADC: %lu\n", bat_voltage_avg, adc_avg);
					  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
				  }

				  if (bat_voltage_avg <= DROPOUT_VOLTAGE) {
					  measured_values1[PACKET_LENGTH - BAT_FIELD_SIZE] = 0xffff;
					  measured_values2[PACKET_LENGTH - BAT_FIELD_SIZE] = 0xffff;
					  if (verbosity >= 1) {
						  sprintf(msg, "V_BAT: %.3f V below %.3f V, flipped battery bits\n", bat_voltage_avg, BAT_LOW_THRESHOLD);
						  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
					  }
				  }

				  bat_voltage_avg = 0;
				  adc_avg = 0;
				  adc_counter = 0;
			  }
		  }

		  if (verbosity >= 2) {
			  sprintf(msg, "tc: %u, tx_due: %u\n", tick_count, tx_due);
			  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
		  }

		  // Wait for device ID times offset with sending to avoid overlapping transmissions at the master node.
		  if ( tx_due && tick_count == (LORA_HEADER_LENGTH - 1 + (device_id * TX_OFFSET)) ) {
			  //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
			  if (flip_buffer) {
				  SX1278_LoRaTxPacket(&SX1278, (uint8_t *) &measured_values1, LORA_PACKET_SIZE, LORA_TIMEOUT);
				  if (verbosity >= 1) { HAL_UART_Transmit(&huart4, (uint8_t *) "sent arr. 1\n", 12, 100); }
			  } else {
				  SX1278_LoRaTxPacket(&SX1278, (uint8_t *) &measured_values2, LORA_PACKET_SIZE, LORA_TIMEOUT);
				  if (verbosity >= 1) { HAL_UART_Transmit(&huart4, (uint8_t *) "sent arr. 2\n", 12, 100); }
			  }

			  //HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
			  tx_due = 0;
		  }

		  for (volatile uint16_t i = 0; i < BUFSIZE; i += 4) {
			  vReal[i/4] = abs(65535 - i2s_dma_buf[i]);
			  vImag[i/4] = 0;
		  }

		  // Alternate between two buffers to allow sending while another buffer is being written to.
		  if (flip_buffer) {
			  measured_values2[tick_count] = GetDBA(vReal, vImag);
			  if (verbosity >= 2) {
				  sprintf(msg, "fill array 1: %u\n", tick_count);
				  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
			  }
		  } else {
			  measured_values1[tick_count] = GetDBA(vReal, vImag);
			  if (verbosity >= 2) {
				  sprintf(msg, "fill array 1: %u\n", tick_count);
				  HAL_UART_Transmit(&huart4, (uint8_t *) msg, strlen(msg), 100);
			  }
		  }

		  // One tick before expecting the sync beacon we switch to Rx. We could've done this earlier
		  // but keeping the LoRa module in Rx mode uses up a lot of power.
		  if (tick_count == ( PACKET_LENGTH - BAT_FIELD_SIZE - 2 ) ) {
			  SX1278_LoRaEntryRx(&SX1278, 4, LORA_TIMEOUT);
			  if (verbosity >= 2) { HAL_UART_Transmit(&huart4, (uint8_t *) "switch to rx\n", 13, 100); }
		  }

		  tick_count++;

		  // TODO This could probably be done in the while !beacon loop.
		  // TODO Update setup values should they change.
		  // All measurements are done and we're waiting for the sync beacon to start the cycle again.
		  while ( __HAL_TIM_GET_COUNTER(&htim2) < (TICK_DURATION_US) || tick_count == (PACKET_LENGTH - BAT_FIELD_SIZE ) ) {
			  ret = SX1278_LoRaRxPacket(&SX1278);

			  if (ret > 0 && ret <= LORA_BEACON_SIZE) {
				  memset(lora_beacon_rx_buffer, 0, sizeof lora_beacon_rx_buffer);
				  SX1278_read(&SX1278, (uint8_t*) lora_beacon_rx_buffer, LORA_BEACON_SIZE);
				  if (verbosity >= 2) { HAL_UART_Transmit(&huart4, (uint8_t *) lora_beacon_rx_buffer, sizeof(lora_beacon_rx_buffer), 100); }

				  // Beacons only come from the master (device_id == 0)
				  if (IsOwnLoraPacket(lora_beacon_rx_buffer) && GetDeviceId(lora_beacon_rx_buffer) == 0) {
					  if (verbosity >= 1) { HAL_UART_Transmit(&huart4, (uint8_t *) "rx beacon\n", 10, 100); }

					  flip_buffer ^= 1;		// Flip the flip_buffer bit.
					  tick_count = LORA_HEADER_LENGTH;

					  uint8_t new_tick_count = lora_beacon_rx_buffer[2];
					  uint32_t new_tick_duration = lora_beacon_rx_buffer[3] * 1000000;
					  uint8_t new_tx_offset = lora_beacon_rx_buffer[4];

					  // If one of these values changes, don't send the last packet as it's not adhering to the new parameters anymore.
					  if (new_tick_count != TICK_COUNT || new_tick_duration != TICK_DURATION_US || new_tx_offset != TX_OFFSET) {
						  tx_due = 0;
						  HAL_UART_Transmit(&huart4, (uint8_t *) "values changed, don't send\n", 27, 100);
					  } else {
						  tx_due = 1;
					  }

					  if (verbosity >= 2) { HAL_UART_Transmit(&huart4, (uint8_t *) "flip\n", 6, 100); }

					  TICK_COUNT = new_tick_count;
					  TICK_DURATION_US = new_tick_duration;
					  TX_OFFSET = new_tx_offset;
					  PACKET_LENGTH = (TICK_COUNT + LORA_HEADER_LENGTH + BAT_FIELD_SIZE);
					  LORA_PACKET_SIZE = PACKET_LENGTH * 2;

					  HAL_UART_Transmit(&huart4, (uint8_t *) "\n", 1, 100);
					  while (__HAL_TIM_GET_COUNTER(&htim2) < (TICK_DURATION_US));	// Some µs might be left.

					  // Switch back to Tx right here as it saves power.
					  SX1278_entryLoRa(&SX1278);
					  SX1278_LoRaEntryTx(&SX1278, LORA_PACKET_SIZE, LORA_TIMEOUT);
					  break;
				  }
			  }
		  }

	  }
	  /*
	  }

	  */
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Configure the main internal regulator output voltage
  */
  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the peripherals clocks
  */
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2S1|RCC_PERIPHCLK_ADC;
  PeriphClkInit.I2s1ClockSelection = RCC_I2S1CLKSOURCE_SYSCLK;
  PeriphClkInit.AdcClockSelection = RCC_ADCCLKSOURCE_SYSCLK;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_ADC1_Init(void)
{

  /* USER CODE BEGIN ADC1_Init 0 */

  /* USER CODE END ADC1_Init 0 */

  ADC_ChannelConfTypeDef sConfig = {0};

  /* USER CODE BEGIN ADC1_Init 1 */

  /* USER CODE END ADC1_Init 1 */
  /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
  */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
  hadc1.Init.Resolution = ADC_RESOLUTION_12B;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  hadc1.Init.LowPowerAutoWait = DISABLE;
  hadc1.Init.LowPowerAutoPowerOff = DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.NbrOfConversion = 3;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.DMAContinuousRequests = ENABLE;
  hadc1.Init.Overrun = ADC_OVR_DATA_PRESERVED;
  hadc1.Init.SamplingTimeCommon1 = ADC_SAMPLETIME_1CYCLE_5;
  hadc1.Init.SamplingTimeCommon2 = ADC_SAMPLETIME_1CYCLE_5;
  hadc1.Init.OversamplingMode = DISABLE;
  hadc1.Init.TriggerFrequencyMode = ADC_TRIGGER_FREQ_HIGH;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_2;
  sConfig.Rank = ADC_REGULAR_RANK_1;
  sConfig.SamplingTime = ADC_SAMPLINGTIME_COMMON_1;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_9;
  sConfig.Rank = ADC_REGULAR_RANK_2;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Regular Channel
  */
  sConfig.Channel = ADC_CHANNEL_VREFINT;
  sConfig.Rank = ADC_REGULAR_RANK_3;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN ADC1_Init 2 */

  /* USER CODE END ADC1_Init 2 */

}

/**
  * @brief I2C2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C2_Init(void)
{

  /* USER CODE BEGIN I2C2_Init 0 */

  /* USER CODE END I2C2_Init 0 */

  /* USER CODE BEGIN I2C2_Init 1 */

  /* USER CODE END I2C2_Init 1 */
  hi2c2.Instance = I2C2;
  hi2c2.Init.Timing = 0x0010061A;
  hi2c2.Init.OwnAddress1 = 120;
  hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c2.Init.OwnAddress2 = 0;
  hi2c2.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
  hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c2) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Analogue filter
  */
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
  {
    Error_Handler();
  }
  /** Configure Digital filter
  */
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C2_Init 2 */
  if (HAL_GPIO_ReadPin(I2C_Address_GPIO_Port, I2C_Address_Pin) == GPIO_PIN_SET) {	// Jumper open, internal pull up
	  hi2c2.Init.OwnAddress1 = 120;
  } else { // Jumper closed, pulled to GND
	  hi2c2.Init.OwnAddress1 = 122;
  }

  if (HAL_I2C_Init(&hi2c2) != HAL_OK)
  {
    Error_Handler();
  }

  /* USER CODE END I2C2_Init 2 */

}

/**
  * @brief I2S1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2S1_Init(void)
{

  /* USER CODE BEGIN I2S1_Init 0 */

  /* USER CODE END I2S1_Init 0 */

  /* USER CODE BEGIN I2S1_Init 1 */

  /* USER CODE END I2S1_Init 1 */
  hi2s1.Instance = SPI1;
  hi2s1.Init.Mode = I2S_MODE_MASTER_RX;
  hi2s1.Init.Standard = I2S_STANDARD_PHILIPS;
  hi2s1.Init.DataFormat = I2S_DATAFORMAT_24B;
  hi2s1.Init.MCLKOutput = I2S_MCLKOUTPUT_DISABLE;
  hi2s1.Init.AudioFreq = I2S_AUDIOFREQ_22K;
  hi2s1.Init.CPOL = I2S_CPOL_LOW;
  if (HAL_I2S_Init(&hi2s1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2S1_Init 2 */

  /* USER CODE END I2S1_Init 2 */

}

/**
  * @brief SPI2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_SPI2_Init(void)
{

  /* USER CODE BEGIN SPI2_Init 0 */

  /* USER CODE END SPI2_Init 0 */

  /* USER CODE BEGIN SPI2_Init 1 */

  /* USER CODE END SPI2_Init 1 */
  /* SPI2 parameter configuration*/
  hspi2.Instance = SPI2;
  hspi2.Init.Mode = SPI_MODE_MASTER;
  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
  hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
  hspi2.Init.NSS = SPI_NSS_SOFT;
  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
  hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi2.Init.CRCPolynomial = 7;
  hspi2.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
  hspi2.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
  if (HAL_SPI_Init(&hspi2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI2_Init 2 */

  /* USER CODE END SPI2_Init 2 */

}

/**
  * @brief TIM2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM2_Init(void)
{

  /* USER CODE BEGIN TIM2_Init 0 */

  /* USER CODE END TIM2_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 16-1;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 4294967295;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */

}

/**
  * @brief USART4 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART4_UART_Init(void)
{

  /* USER CODE BEGIN USART4_Init 0 */

  /* USER CODE END USART4_Init 0 */

  /* USER CODE BEGIN USART4_Init 1 */

  /* USER CODE END USART4_Init 1 */
  huart4.Instance = USART4;
  huart4.Init.BaudRate = 115200;
  huart4.Init.WordLength = UART_WORDLENGTH_8B;
  huart4.Init.StopBits = UART_STOPBITS_1;
  huart4.Init.Parity = UART_PARITY_NONE;
  huart4.Init.Mode = UART_MODE_TX;
  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART4_Init 2 */

  /* USER CODE END USART4_Init 2 */

}

/**
  * Enable DMA controller clock
  */
static void MX_DMA_Init(void)
{

  /* DMA controller clock enable */
  __HAL_RCC_DMA1_CLK_ENABLE();

  /* DMA interrupt init */
  /* DMA1_Channel1_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
  /* DMA1_Channel2_3_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(NSS_GPIO_Port, NSS_Pin, GPIO_PIN_SET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, LED_Pin|RESET_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pins : NSS_Pin LED_Pin RESET_Pin */
  GPIO_InitStruct.Pin = NSS_Pin|LED_Pin|RESET_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pin : PB0 */
  GPIO_InitStruct.Pin = GPIO_PIN_0;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  /*Configure GPIO pin : I2C_Address_Pin */
  GPIO_InitStruct.Pin = I2C_Address_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(I2C_Address_GPIO_Port, &GPIO_InitStruct);

}

/* USER CODE BEGIN 4 */
void HAL_I2C_AddrCallback(I2C_HandleTypeDef *hi2c, uint8_t TransferDirection, uint16_t AddrMatchCode) {

	HAL_I2C_DisableListen_IT(hi2c);
	if (TransferDirection == I2C_TX) {
		HAL_I2C_Slave_Transmit_IT(hi2c, (uint8_t *) &i2c_tx_buffer[i2c_tx_counter * PACKET_LENGTH], LORA_PACKET_SIZE);
		i2c_tx_counter++;
	} else if (TransferDirection == I2C_RX) {
		HAL_I2C_Slave_Receive_IT(hi2c, (uint8_t *) i2c_rx_buffer, sizeof(i2c_rx_buffer));	// The Arduino lib sends an extra byte in the beginning.
	}

}

void HAL_I2C_SlaveTxCpltCallback(I2C_HandleTypeDef *hi2c) {
	if (i2c_tx_counter == DEVICE_COUNT) {
		i2c_tx_counter = 0;
		memset(i2c_tx_buffer, 0, sizeof i2c_tx_buffer);
	}
	HAL_I2C_EnableListen_IT(hi2c);
}

void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef *hi2c) {
	got_i2c_setup_message = 1;
	HAL_I2C_EnableListen_IT(hi2c);
}

void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {
	adc_ready = 1;
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/