// CCurvatura.cpp. Implementación de la clase CCurvatura

#include "stdafx.h"
#include "resource.h"
#include <math.h>
#include "CCurvatura.h"
#include "funciones.h"

#define MAXCONTORNO 2048
#define T 2049 				// MAXCONTORNO + 1
#define TPRIMA 2048			// MAXCONTORNO
#define T2 4097				// (2 * MAXCONTORNO) + 1
#define PI 3.14159265
 

// Constructores
CCurvatura::CCurvatura()
{
	num_puntos = 0;
	num_intervalos = 0;
}

CCurvatura::CCurvatura(int num_ptos)
{
	num_puntos = num_ptos;
	num_intervalos = 0;
	
	// Reservo memoria para la curvatura
	k = (double *) malloc((size_t) (sizeof(double) * num_puntos));
}

CCurvatura::~CCurvatura()
{
	free(k);			// Libero la memoria ocupada por 'k'
	free(intervalos);	// Libero la memoria ocupada por 'intervalos'
}

int CCurvatura::CalcularCurvatura(float vx[], float vy[], int NptosContor, double sigma, int maxinter, int ui, int us, int punto)
{
	int i, ind;
	double *tmp;

	// Actualizamos los valores de los umbrales
	umbral_inf = (ui * -PI) / 100.0;
	umbral_sup = (us * +PI) / 100.0;
	
	// Calculo la función de curvatura
	Lcurvatura(vx, vy, NptosContor, sigma);
	
	// Desplazamos el vector k en punto-posiciones ya que hemos calculado
	// la curvatura a partir de punto (porque no queremos comenzar en un pico)
	// pero el resultado hay que mostrarlo desde 0 y no desde punto, ya que
	// punto solo nos ha servido para poder calcular bien la curvatura
	if (punto != 0)
	{
		// Reservo memoria para la curvatura
		tmp = (double *) malloc((size_t) (sizeof(double) * num_puntos));

		for (i=0;i<num_puntos;i++)
			tmp[i] = k[i];
		
		for (i=0;i<num_puntos;i++)
		{
			if ((i - punto) < 0)
				ind = num_puntos - punto + i;
			else
				ind = i - punto;

			k[i] = tmp[ind];
		}

		delete tmp;
	}
	
	// Calculamos los intervalos de esa función de curvatura
	if (ObtenerIntervalos(maxinter) == 1)
		return 1; // Error: Demasiados intervalos
	
	if (num_intervalos == 0)
		return 2; // Error: Cero intervalos

	// Calculamos todos los puntos caracteristicos de cada uno de los intervalos
	ObtenerPuntosCaracteristicos();
	
	return 0; // No ha habido errores
}

void CCurvatura::Lcurvatura(float vx[], float vy[], int NptosContor, double sigma)
{
	double d1[T2],d2[T2],xc[MAXCONTORNO],yc[MAXCONTORNO],xc2[MAXCONTORNO],yc2[MAXCONTORNO];
	int i,cociente;

	// Calculo las derivadas 1ª y 2ª de la función densidad con la sigma correspondiente
	Derivadas(sigma, d1 - 1, d2 - 1);
    // Calculo las 1ªs derivadas de vx y vy
	Convoluciona1(vx, d1, xc, NptosContor);
    Convoluciona1(vy, d1, yc, NptosContor);
    // Calculo las 2ªs derivadas de vx y vy
	Convoluciona1(vx, d2, xc2, NptosContor);
    Convoluciona1(vy, d2, yc2, NptosContor);

	// Calculo la función de curvatura (k) para todos los puntos del contorno
	for(i=0;i<NptosContor;i++)
    {
		k[i] = (xc2[i] * yc[i] - xc[i] * yc2[i]) /
				pow((double) xc[i] * xc[i] + yc[i] * yc[i], (double) 1.5);
	    if(fabs((double) k[i]) > PI)
		{
			cociente = (int) (k[i] / PI);
			k[i] = k[i] - cociente * PI;
		}
    }
}

void CCurvatura::Convoluciona1(float x[], double resp[], double dev[], int N)
{	
	register int t;
	double entrada[MAXCONTORNO], salida[T2], respinter[MAXCONTORNO];

	for(t=0;t<N;t++)
	{
		respinter[t] = (float) resp[t];
		entrada[t] = x[t];
	}
	
	for(t=N;t<MAXCONTORNO;t++)
	{
		respinter[t] = resp[t];
		entrada[t] = entrada[N-1];
	}
	
	for(t=0;t<T2;t++)
		salida[t] = 0.0;

	// Realizamos la derivada
	Convlv((entrada - 1), TPRIMA, (respinter - 1), TPRIMA, 1, (salida - 1));
	
	for(t=0;t<N;t++)
		dev[t] = salida[t];
}

void CCurvatura::Derivadas(double sigma, double d1g[], double d2g[])
{	
	long int t;
	long double A, B, C, D;

	C = sigma * sigma;
	B = 2 * C;
	A = sigma * C * sqrt( (double) (2 * PI));
	
	for(t=0;t<(T-1)/2;t++)
	{
		D = exp( (double) (-t * t / B));
		d1g[t + 1] = (double) -t * D / A;
		d1g[T - t] = (double) -d1g[t + 1];
		d2g[T - t] = d2g[t + 1] = (double) (-1 + t * t / C) * D / A;
	};
}

static double sqrarg;
#define SQR(a) (sqrarg=(a),sqrarg*sqrarg)

void CCurvatura::Convlv(double data[], int n, double respns[], int m, int isign, double ans[])
{	
	int i, no2;
    long double dum, mag2;
	double fft[T2];

	for (i=1;i<=(m-1)/2;i++)
		respns[n + 1 - i] = respns[m + 1 - i];

	for (i=(m+3)/2;i<=n-(m-1)/2;i++)
		respns[i] = 0.0;

	Twofft(data, respns, fft, ans, n);
	no2 = n / 2;

	for (i=2;i<=n+2;i+=2)
	{
		if (isign == 1)
		{
        	ans[i - 1] = (fft[i - 1] * (dum = ans[i - 1]) - fft[i] * ans[i]) / no2;
			ans[i] = (fft[i] * dum + fft[i-1] * ans[i]) / no2;
		}
		else if (isign == -1)
		{
 			if ((mag2 = SQR(ans[i - 1]) + SQR(ans[i])) == 0.0)
				ans[i - 1] = (fft[i-1] * (dum = ans[i-1]) + fft[i] * ans[i]) / mag2 / no2;
			
			ans[i] = (fft[i] * dum - fft[i-1] * ans[i]) / mag2 / no2;
		}
	}
	
	ans[2] = ans[n + 1];
	Realft(ans, no2, -1);
}

#undef SQR

void CCurvatura::Twofft(double data1[], double data2[], double fft1[], double fft2[], int n)
{	
	int nn3, nn2, jj, j;
	double rep, rem, aip, aim;

	nn3 = 1 + (nn2 = 2 + n + n);
	for (j=1,jj=2;j<=n;j++,jj+=2)
	{
		fft1[jj - 1] = data1[j];
		fft1[jj] = data2[j];
	}
	
	Four1(fft1,n,1);

	fft2[1] = fft1[2];
	fft1[2] = fft2[2] = 0.0;

	for (j=3;j<=n+1;j+=2)
	{
		rep=0.5 * (fft1[j] + fft1[nn2 - j]);
		rem=0.5 * (fft1[j] - fft1[nn2 - j]);
		aip=0.5 * (fft1[j + 1] + fft1[nn3 - j]);
		aim=0.5 * (fft1[j + 1] - fft1[nn3 - j]);
		fft1[j] = rep;
		fft1[j + 1] = aim;
		fft1[nn2 - j] = rep;
	 	fft1[nn3 - j] = -aim;
		fft2[j] = aip;
		fft2[j + 1] = -rem;
		fft2[nn2 - j] = aip;
		fft2[nn3 - j] = rem;
	}
}
 
#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr

void CCurvatura::Four1(double data[], int nn, int isign)
{	
	int n, mmax, m, j, istep, i;
	double wtemp, wr, wpr, wpi, wi, theta, tempr, tempi;

	n = nn << 1;
	j = 1;
	for (i=1;i<n;i+=2)
	{
		if (j > i)
		{
			SWAP(data[j],data[i]);
			SWAP(data[j + 1],data[i + 1]);
		}
		
		m = n >> 1;
		
		while (m >= 2 && j > m)
		{
			j -= m;
			m >>= 1;
		}
		j += m;
	}
	
	mmax = 2;
	while (n > mmax)
	{
		istep = 2 * mmax;
		theta = 6.28318530717959 / (isign * mmax);
		wtemp = sin(0.5 * theta);
		wpr = -2.0 * wtemp * wtemp;
		wpi = sin(theta);
		wr = 1.0;
		wi = 0.0;
		for (m=1;m<mmax;m+=2)
		{
			for (i=m;i<=n;i+=istep)
			{
				j = i + mmax;
				tempr = wr * data[j] - wi * data[j + 1];
				tempi = wr *data[j + 1] + wi * data[j];
				data[j] = data[i] - tempr;
				data[j + 1] = data[i + 1] - tempi;
				data[i] += tempr;
				data[i + 1] += tempi;
			}
			wr = (wtemp = wr) * wpr - wi * wpi + wr;
			wi = wi * wpr + wtemp * wpi + wi;
		}
		
		mmax = istep;
	}
}

#undef SWAP

void CCurvatura::Realft(double data[], int n, int isign)
{	
	int i, i1, i2, i3, i4, n2p3;
	double c1=0.5, c2, h1r, h1i, h2r, h2i, wr, wi, wpr, wpi, wtemp, theta;

	theta = PI / (double) n;
	if (isign == 1)
	{
		c2 = -0.5;
		Four1(data, n, 1);
	}
	else
	{
		c2 = 0.5;
		theta = -theta;
	}
	
	wtemp = sin(0.5 * theta);
	wpr = -2.0 * wtemp * wtemp;
	wpi = sin(theta);
	wr = 1.0 + wpr;
	wi = wpi;
	n2p3 = 2 * n + 3;
	for (i=2;i<=n/2;i++)
	{
		i4 = 1 + (i3= n2p3 - (i2 = 1 + (i1 = i + i - 1)));
		h1r = c1 * (data[i1] + data[i3]);
		h1i = c1 * (data[i2] - data[i4]);
		h2r = -c2 * (data[i2] + data[i4]);
		h2i = c2 * (data[i1] - data[i3]);
		data[i1] = h1r + wr * h2r - wi * h2i;
		data[i2] = h1i + wr * h2i + wi * h2r;
		data[i3] = h1r - wr * h2r + wi * h2i;
		data[i4] = -h1i + wr * h2i + wi * h2r;
		wr = (wtemp = wr) * wpr - wi * wpi + wr;
		wi = wi * wpr + wtemp * wpi + wi;
	}

	if (isign == 1)
	{
		data[1] = (h1r = data[1]) + data[2];
		data[2] = h1r - data[2];
	}
	else
	{
		data[1] = c1 * ((h1r = data[1]) + data[2]);
	    data[2] = c1 * (h1r - data[2]);
	    Four1(data, n, -1);
	}
}

int CCurvatura::ObtenerIntervalos(int maxinter)
{
	int ind, i, estado; // 0 = normal. 1 = Dentro de un intervalo positivo
						// 2 = Dentro de un intervalo negativo
	
	// Asignamos memoria al vector de intervalos de curvatura (inicializando a 0)
	intervalos = (CIntervalo *) calloc(maxinter + 1, sizeof(CIntervalo));
	
	if (k[0] >= umbral_sup)
	{
		estado = 1;				   // Comienzo de intervalo positivo
		intervalos[0].inicio = -1; // No hay inicio aún
		intervalos[0].signo = 0;   // Iniciamos un intervalo positivo
	}
	else if (k[0] <= umbral_inf)
	{
		estado = 2;				   // Comienzo de intervalo negativo
		intervalos[0].inicio = -1; // No hay inicio aún
		intervalos[0].signo = 1;   // Iniciamos un intervalo negativo
	}
	else
	{
		estado = 0;				  // Estamos fuera de cualquier intervalo
		intervalos[0].inicio = 0; // No estamos en ningún intervalo
	}

	ind = 0;
	for (i=0;i<num_puntos;i++)
	{
		if ((estado == 0) && (k[i] >= umbral_sup)) // Comienzo interv. +
		{
			if (ind >= maxinter)
				return 1; // No puede haber tantos intervalos

			intervalos[ind].inicio = i;  // Guardamos el valor de inicio
			intervalos[ind].fin = -1;	 // Desconocemos aún el fin
			intervalos[ind].signo = 0;   // Iniciamos un intervalo positivo
			estado = 1;
		}
		else if ((estado == 0) && (k[i] <= umbral_inf)) // Comienzo interv. -
		{
			if (ind >= maxinter)
				return 1; // No puede haber tantos intervalos

			intervalos[ind].inicio = i;	 // Guardamos el valor de inicio
			intervalos[ind].fin = -1;	 // Desconocemos aún el fin
			intervalos[ind].signo = 1;   // Iniciamos un intervalo negativo
			estado = 2;
		}
		else if (((estado == 1) && (k[i] < umbral_sup)) || // Fin intervalo
				 ((estado == 2) && (k[i] > umbral_inf)))
		{
			intervalos[ind].fin = i - 1; // Cerramos un intervalos (pos. o neg.)
			estado = 0;
			ind++;
		}	
	}
	
	if ((intervalos[ind].inicio == 0) && (intervalos[ind].fin == 0))
	{	// No se ha llegado a utilizar este intervalo
		if (intervalos[0].inicio == -1)
			// Si, además, el primer interv. tenia de inicio -1, porque partia con curvtura>umbral,
			// como no se ha generado otro intervalo, es porque el inicio era el primer punto (0).
			intervalos[0].inicio = 0;
	}
	else if (intervalos[ind].fin == -1)
	{
		if (intervalos[0].inicio == -1)
			intervalos[0].inicio = intervalos[ind].inicio;
		else
		{
			intervalos[ind].fin = num_puntos - 1;
			ind++;
		}
	}
	
	num_intervalos = ind;

	return 0; // No hay errores
}

void CCurvatura::ObtenerPuntosCaracteristicos()
{
	int i, punto_ant;
	double max_curv = 0, val;

	for (i=0;i<num_intervalos;i++)
	{
		// Obtenemos el punto central de cada intervalo, es decir, el punto
		// máximo de un intervalo + y el punto mínimo de un intervalo -
		if (intervalos[i].inicio == intervalos[i].fin)
			intervalos[i].punto = intervalos[i].inicio;
		else
		{
			if (intervalos[i].signo == 0) // Si es positivo
				intervalos[i].punto = MaximoPunto(i);
			else						  // Si es negativo
				intervalos[i].punto = MinimoPunto(i);
		}
		
		// Calculamos el valor real de la curvatura en % respecto a PI
		intervalos[i].val_curv = Aproximar(fabs(k[intervalos[i].punto] * 100.0 / PI));

		// Voy calculando el punto de máxima curvatura (lo guardo en max_curv)
		val = fabs(k[intervalos[i].punto]);		
		if (val > max_curv)
			max_curv = val;

		if (i != 0) // A partir del segundo intervalo
		{
			// Guardo la distancia real del punto i - 1 al punto i
			intervalos[i - 1].dist_sigr = Distancia(punto_ant,intervalos[i].punto);
			// Guardo la distancia relativa a la dist. total del punto i - 1 al punto i
			intervalos[i - 1].dist_sig = Aproximar((double) Distancia(punto_ant,intervalos[i].punto)
									   * 100.0 / (double) num_puntos); 
		}

		// Guardo este punto como punto_ant para usarlo en la siguiente iteración del bucle
		punto_ant = intervalos[i].punto;
	}

	// Guardo las distancias reales y relativas del último punto al punto primero
	intervalos[num_intervalos - 1].dist_sigr =
			Distancia(intervalos[num_intervalos - 1].punto,intervalos[0].punto);
	intervalos[num_intervalos - 1].dist_sig = 
			Aproximar((double) Distancia(intervalos[num_intervalos - 1].punto,intervalos[0].punto)
									   * 100.0 / (double) num_puntos);
}

int CCurvatura::MaximoPunto(int interv)
{
	int i, maximo;
	double val_maximo = -PI;

	i = intervalos[interv].inicio;
	while (i != intervalos[interv].fin + 1)
	{
		i = i % num_puntos;

		if (k[i] > val_maximo)
		{
			val_maximo = k[i];
			maximo = i;
		}

		i = i + 1;
	}
	
	return maximo;
}

int CCurvatura::MinimoPunto(int interv)
{
	int i, minimo;
	double val_minimo = PI;

	i = intervalos[interv].inicio;
	while (i != intervalos[interv].fin + 1)
	{
		i = i % num_puntos;

		if (k[i] < val_minimo)
		{
			val_minimo = k[i];
			minimo = i;
		}

		i = i + 1;
	}
	
	return minimo;
}

int CCurvatura::Distancia(int p1, int p2)
{
	int res;

	if (p1 < p2)
		res = p2 - p1;
	else
		res = (num_puntos - p1) + p2;

	return res;
}

int CCurvatura::ObtenerPuntoLiso(int rango)
{
	int pto, i, intini, intfin;
	bool fin;

	fin = false;
	pto = (rango - 1) / 2;
	i = rango; // Para no empezar en el origen
	
	while ((i < num_puntos) && (!fin))
	{
		if ((i - pto) < 0)
			intini = num_puntos - pto + i;
		else
			intini = i - pto;

		intfin = (i + pto) % num_puntos;

		fin = IntervaloPlano(intini, intfin);

		if (!fin)
			i++;
	}
	
	return i;
}

bool CCurvatura::IntervaloPlano(int intini, int intfin)
{
	int i;
	bool fin;

	fin = false;
	i = intini;

	while ((i != intfin + 1) && (!fin))
	{
		if ((k[i] >= umbral_sup) || (k[i] <= umbral_inf))
			fin = true;
		else
			i = (i + 1) % num_puntos;
	}
	
	return !fin;
}
		
//	while ((pto < num_puntos) && (res == 0))
//	{
//		fin = false;
//		i = pto;
//
//		while ((i < pto + rango) && (!fin))
//		{
//			if ((k[i] >= umbral_sup) || (k[i] <= umbral_inf))
//				fin = true;
//			else
//				i++;
//		}
//
//		if (fin)
//			pto = i + 1;
//		else
//			res = pto + ((rango - 1) / 2);
//	}
//
//	if (pto >= num_puntos)
//		return -1;
//	
//	return res;








