/******************************************************************************\
*    This is part from DSP - Library for Parallax Propeller 2 µC               *
*    init February 2023 by Reinhard Mayer                                      *
\******************************************************************************/

#include "complex.h"

void generate_sin_cos (int * buffer, int N, int amplitude, int phi_0 , int speed, short cos_flag)
{
  int abs = amplitude;
  int phi = phi_0;
  int _cos,_sin;
  int dphi = 2*CORDIC_PI / N;  
	for(int n=0; n < N; n++)
	{
		toCartesian(&_cos, &_sin, abs, phi);
    if(cos_flag == 1)
		  buffer[n] = _cos;
    else
      buffer[n] = _sin;
		phi = phi + speed * dphi;
	}  
}

void dft (int * buffer, int N, int * X_abs, int * X_arg)
{

  int real=0,imag=0,_abs,_arg;
  int phi = 0;
  int dphi = 2 * CORDIC_PI / N;

	for (int k = 0; k < N; k++)
	{
		X_real[k] = 0; X_imag[k] = 0;
		for(int n = 0; n < N; n++)
		{
			toCartesian(&real, &imag, buffer[n],-k * phi);
			X_real[k] += real;
			X_imag[k] += imag;
			phi += dphi;		
		}
    phi = 0;
		toPolar(&_abs,&_arg,X_real[k],X_imag[k]);
		X_abs[k] = _abs;
		X_arg[k] = _arg;
    
  }  
}

void hilbert (int * buffer, int N, int * X_abs, int * X_arg)
{

  int real=0,imag=0,_abs,_arg;
  int phi = 0;
  int dphi = 2 * CORDIC_PI / N;

	for (int k = 0; k < N; k++)
	{
		X_real[k] = 0; X_imag[k] = 0;
		for(int n = 0; n < N; n++)
		{
			toCartesian(&real, &imag, buffer[n],-k * phi);
			X_real[k] += real;
			X_imag[k] += imag;
			phi += dphi;		
		}
    phi = 0;
		toPolar(&_abs,&_arg,X_real[k],X_imag[k]);
		X_abs[k] = _abs;
    if(k < N/2)
      _arg = rot90degree_cw ( _arg);
    else
       _arg = rot90degree_ccw ( _arg);
		X_arg[k] = _arg;
    
  }  
}



void inversdft (int * y, int N, int * X_abs, int * X_arg)
{
  #define scale 100
  int phi,a,b,c,d;
  int dphi = 2 * CORDIC_PI / N;
	phi = 0;
	for (int k = 0; k < N; k++)
	{
		y[k] = 0;
		for(int n = 0; n < N; n++)
		{
			toCartesian(&a, &b, scale, k * phi);
			toCartesian(&c, &d, X_abs[n],X_arg[n]);
			y[k] += a * c  - b * d;
      phi  += dphi;
		}
		y[k] = y[k] / (scale * N);
		phi = 0;
	}
}


void toPolar (int * abs, int * arg, int real, int imag)
{
	int _abs,_arg;
	__asm {
			qvector	real,imag
			getqx	_abs
			getqy	_arg
	};
	* abs = _abs;
	* arg = _arg;
}

void toCartesian(int * real, int * imag, int _abs, int _arg)
{
	int _real,_imag;
	__asm {
			qrotate	_abs, _arg
			getqx	_real
			getqy	_imag
	};
	*real = _real;
	*imag = _imag;
}

void rot (int * real, int * imag, int phi)
{
	int x = *real;
	int y = *imag;
	__asm {
			setq y
			qrotate	x,phi
			getqx	x
			getqy 	y
	};
	*real = x;
	*imag = y;
}

int rot90degree_cw (int arg)
{
	return ( arg - CORDIC_PI_2);
}

int rot90degree_ccw (int arg)
{
	return ( arg + CORDIC_PI_2);
}