/* Includes ------------------------------------------------------------------*/
#include "stm32f3xx_hal.h"
#include <math.h>



/* USER CODE BEGIN Includes */
#include "led_driver.h"
#include "image_generator.h"
#include "globals.h"

/* USER CODE END Includes */

// Prend une image rgb raw, la filtre passe bas à plus haute résolution
// filtrage gaussien
// Probleme de la conservation de la couleur...
// uint32_t getRawOffset(uint32_t column,uint32_t line)

// version qui accepte aussi les valeurs négatives, mais pas au delà de -
uint32_t getRawOffsetWRollOver(int32_t column,int32_t line)
{
	if (column<0)
		column+=NB_COLUMNS;
	if (line<0)
		line+=NB_LINES;
	return 3*((column % NB_COLUMNS)+(line % NB_LINES)*NB_LEDS_PER_STRIP);
}

// assumes	 -NB_COLUMNS<column<2*NB_COLUMNS
inline uint32_t getRawOffsetXWRollOver(int32_t column,int32_t line)
{
	if (column<0)
		column+=NB_COLUMNS;
	else if (column>=NB_COLUMNS)
		column-=NB_COLUMNS;

	return 3*(column+line*NB_COLUMNS);
}

__attribute__((section("ccmram")))

float fonctionGaussienne(float sigma, float x)
{
	return 1.0f/(sqrt(2*M_PI)*sigma)*exp(-x*x/(2*sigma*sigma));
}

__attribute__((section("ccmram")))
// nb coeff doit être impair
void calculeCoeffsGaussienne(float *coeffs, int nbcoeff, float step, float sigma, float offset)
{
	int i;
	int minimax = (nbcoeff-1)/2;
	float total;
	float *tmpCoeffs;
	
	total = 0;
	tmpCoeffs = coeffs;
	
	for (i=-minimax;i<=minimax;i++)
	{
		*tmpCoeffs = fonctionGaussienne(sigma,i*step+offset);
		if (*tmpCoeffs<0.01f)
			*tmpCoeffs = 0;
		total+=*tmpCoeffs;
		tmpCoeffs++;
	}
	for (i=-minimax;i<=minimax;i++)
	{
		*coeffs++/=total;
	}	
}

__attribute__((section("ccmram")))
// offsetx doit être limité à -0.5,+0.5
void filtreGaussienX(float sigma, uint16_t sizexin, uint16_t sizeyin, uint8_t *raw_in, uint8_t *raw_out, float offsetx)
{
	int32_t column, line;
	float gCoeffsX[5];
	uint32_t rawOffset;
	float tmpColor[3];
	int i,n;
	
	calculeCoeffsGaussienne(gCoeffsX,5,1.0f,sigma,offsetx);
	
	// Passe horizontale uniquement	
	n=0;
	
	for (line=0;line<NB_LINES;line++)
	{
		for (column=0;column<NB_COLUMNS;column++)
		{
			tmpColor[0] = tmpColor[1] = tmpColor[2] = 0;
			for (i=-2;i<=2;i++)
			{
				rawOffset = getRawOffsetXWRollOver(column+i,line);
				tmpColor[0]+=gCoeffsX[i+2]*raw_in[rawOffset++];
				tmpColor[1]+=gCoeffsX[i+2]*raw_in[rawOffset++];
				tmpColor[2]+=gCoeffsX[i+2]*raw_in[rawOffset];
			}
			raw_out[n] = tmpColor[0];
			raw_out[n+1] = tmpColor[1];
			raw_out[n+2] = tmpColor[2];
			n+=3;
		}
	}
}