/*
    MathComplexArray - Template for complex number array classes
    
    Copyright (C) 1995, Adam Fedor
    
    Should run this through a preprocessor each time defining the
    macro MATH_ORDER to be a number from 1 to 2.
    
    $Id: MathComplexArray.m,v 1.21 2005/04/17 21:22:16 fedor Exp $
*/
# line 12 "MathComplexArray.m"

#ifndef GNUSTEP
#import <Foundation/Foundation.h>
#else
#include <Foundation/NSString.h>
#include <Foundation/NSNotification.h>
#endif
#include <math.h>
#include <stdlib.h>
#include "MathArrayPrivate.h"
#include "MathArray/MAValue.h"
#include "MathArray/MAValueData.h"
#include "MathArray/array_encoding.h"
#include "MathArray/complex.h"
#include "MathComplexArrayPrivate.h"

/* Note: precsion is defined in terms of three words: 0xaabbcc, where cc
   is the storage size of the number. bb is 1 if signed and aa is an
   arbitrary order that increases for more "precise" numbers */
#if MATH_ORDER == 1
#  define MathComplexArray	MathComplexFloatArray
#  define MATH_TYPE		complex_float
#  define MATH_REAL_TYPE	float
#  define MATH_CAST_METHOD	complexFloatValue
static precision_t precision = 	0x400110;
#elif MATH_ORDER == 2
#  define MathComplexArray	MathComplexDoubleArray
#  define MATH_TYPE		complex_double
#  define MATH_REAL_TYPE	double
#  define MATH_CAST_METHOD	complexDoubleValue
static precision_t precision = 	0x500120;
#endif

static void fourn(MATH_REAL_TYPE *data, const unsigned *nn, 
	int ndim, int isign);
typedef MATH_TYPE (*caster_f_t)(void *, unsigned long);
static caster_f_t cast_function(const char *);

@interface MAMutableValueData(MutableData)
- (void) setLength: (unsigned)length;
@end

@interface MathComplexArray : MathArray
{
}

@end

@implementation MathComplexArray : MathArray

+ (precision_t)precision
{
    return precision;
}

+ (const char *)objCType
{
    return @encode(MATH_TYPE);
}

- convertFromObjCType:(const char *)old_type
{
    ordered_index_t i, max_elements;
    const char* newType;
    MATH_TYPE *data;
    caster_f_t caster = cast_function(old_type);
		
    newType = [self objCType];
    max_elements = array_num_elements(dimension, [size bytes]);
    if (array_aligned_sizeof_elements(old_type) 
    		< array_aligned_sizeof_elements(newType)) {
        [arrayData setLength: max_elements * math_aligned_size(newType)];
	data = [arrayData  mutableBytes];
	for (i=0; i < max_elements; i++)
    	    data[max_elements-i-1] = caster(data, max_elements-i-1);
    } else {
	data = [arrayData  mutableBytes];
	for (i=0; i < max_elements; i++)
    	    data[i] = caster(data, i);
        [arrayData setLength: max_elements * math_aligned_size(newType)];
    }
    [arrayData setObjCType: newType];
    return self;
}

- maMatrixMultiply:(MathArray *)otherArray
{
    BOOL odd;
    unsigned i, j, k;
    unsigned a_size[2];
    unsigned b_size[2];
    void *b_data;
    MAMutableValueData *cMatrix;
    MATH_TYPE *a_data;
    MATH_TYPE *c_data;
    MATH_TYPE (*caster)(void *, unsigned long) 
    		= cast_function([otherArray objCType]);

    [super maMatrixMultiply:otherArray];
    a_size[0] = [self sizes][0];
    if (dimension == 2)
    	a_size[1] = [self sizes][1];
    else
    	a_size[1] = 1;
    b_size[0] = [otherArray sizes][0];
    if ([otherArray dimension] == 2)
    	b_size[1] = [otherArray sizes][1];
    else
    	b_size[1] = 1;
    cMatrix = [MAMutableValueData dataWithCount:a_size[0]*b_size[1]
    		objCType:[self objCType]];
    a_data = [arrayData mutableBytes];
    b_data = [[otherArray mathData] mutableBytes];
    c_data = [cMatrix mutableBytes];
    odd = (a_size[1] & 0x1);
    for (i=0; i < a_size[0]; i++) {
      for (j=0; j < b_size[1]; j++) {
	unsigned loc = i*b_size[1]+j;
	c_data[loc].real = 0; c_data[loc].imag = 0;
	for (k=0; k < a_size[1]; k++) {
// FIXME: should we just cast to double since all ops are on double anyway?
#if MATH_ORDER == 1
	  c_data[loc] = c_d2f( c_add( c_f2d(c_data[loc]), 
	  	c_mult(c_f2d(a_data[i*a_size[1]+k]), 
		       c_f2d(caster(b_data, k*b_size[1]+j))) ) );
#else
	  c_data[loc] = c_add(c_data[loc], 
	  	c_mult(a_data[i*a_size[1]+k], caster(b_data, k*b_size[1]+j) ));
#endif
		
#ifdef MATH_CHECK_FPE
	if (!finite(c_data[loc].real) || !finite(c_data[loc].imag))
	    ma_fpe_errno = FLT_OVERFLOW;
#endif
	}
      }
    }

    [arrayData release];
    arrayData = [cMatrix retain];
    a_size[1] = b_size[1];
    if (a_size[1] == 1)
    	dimension = 1;
    [size release];
    size = [[MAValueData dataWithValues:a_size
    		count:dimension
		objCType:@encode(unsigned)] retain];
    return self;
}

- _maOperate:(ma_operator_t)operator with:(MathArray *)value
{
    unsigned long i, j, max_elements;
    unsigned long increment;
    void *otherData;
    MATH_TYPE *data;
    operate_f_t operate = operate_function(operator);
    caster_f_t caster = cast_function([value objCType]);
	
    if (operator == ma_matrix_multiply) {
    	return [self maMatrixMultiply:value];
    }
    if (operate == 0) {
    	MA_RAISE_ILLEGAL;
	/* NOT REACHED */
    }
    	
    data = [arrayData mutableBytes];
    increment = 1;
    otherData = [[value mathData] mutableBytes];
    if ([value dimension] == 0)
	increment = 0;
    max_elements = array_num_elements(dimension, [size bytes]);
    j = 0;
    for (i=0; i < max_elements; i++) {
// FIXME: should we just cast to double since all ops are on double anyway?
#if MATH_ORDER == 1
	MATH_TYPE num;
    	complex_double num1, num2;
	num1 = complexf2d(data, i);
	num = caster(otherData, j);
	num2 = complexf2d(&num, 0);
	num1 = operate(num1, num2);
    	data[i] = complexd2f( &num1, 0);
#else
    	data[i] = operate(data[i], caster(otherData, j));
#endif
#ifdef MATH_CHECK_FPE
	if (!finite(data[i].real) || !finite(data[i].imag))
	    ma_fpe_errno = FLT_OTHER;
#endif
	j += increment;
    }
    return self;
}

- _maPerform: (double_func_t)mathFunction 
{
    unsigned long i, max_elements;
    MATH_TYPE *data;
    complex_func_t cmathFunc = replace_function(mathFunction);

    if (cmathFunc == 0) {
    	MA_RAISE_ILLEGAL;
	/* NOT REACHED */
    }
		
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData  mutableBytes];
    for (i=0; i < max_elements; i++) {
#if MATH_ORDER == 1
	complex_double num;
	num = cmathFunc(complexf2d(data, i));
    	data[i] = complexd2f(&num, 0);
#else
    	data[i] = cmathFunc(data[i]);
#endif
#ifdef MATH_CHECK_FPE
	if (!finite(data[i].real) || !finite(data[i].imag))
	    ma_fpe_errno = FLT_OTHER;
#endif
    }
    return self;
}

- _maPerformFunction:(perform_func_t)perform_func userInfo:(void *)info
{
    unsigned long i, max_elements;
    unsigned  *index = [[[size mutableCopy] autorelease] mutableBytes];
    MATH_TYPE *data;
    NSRange   *range;
    
    range = [[MAMutableValueData dataWithCount:dimension 
		objCType:@encode(NSRange)] mutableBytes];

    for (i=0; i < dimension; i++) {
        range[i].location = 0;
	range[i].length = ((unsigned *)[size bytes])[i];
    }
    index = start_index_from_range(dimension, range, index);
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData mutableBytes];
    // FIXME: need to set up autorelease pool to get rid of extra objects
    for (i=0; i < max_elements; i++) {
        id <NSNumber,ComplexNumber> number;
	number = (id <NSNumber,ComplexNumber>)[arrayData valueAtIndex:i];
    	number = perform_func(number, index, info);
	data[i] = [number MATH_CAST_METHOD];
#ifdef MATH_CHECK_FPE
	if (!finite(data[i].real) || !finite(data[i].imag))
	    ma_fpe_errno = FLT_OTHER;
#endif
	increment_index_in_range(dimension, range, index, 1);
    }
    return self;
}

/* Calulate complex absolute value, store in the real part and then
   call maReal to cast ourselves to a real type */
- maAbs
{
    unsigned long i, max_elements;
    MATH_TYPE *data;

    [MathArray _startMath];
    [self _updateLazyArray];
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData  mutableBytes];
    for (i=0; i < max_elements; i++) {
#if MATH_ORDER == 1
    	data[i].real = c_abs(complexf2d(data, i));
#else
    	data[i].real = (double)c_abs(data[i]);
#endif
    }
    [MathArray _finishMath];
    return [self maReal];
}

- maMagnitude
{
    return [self maAbs];
}

- maPhase
{
    unsigned long i, max_elements;
    MATH_TYPE *data;

    [MathArray _startMath];
    [self _updateLazyArray];
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData  mutableBytes];
    for (i=0; i < max_elements; i++) {
	if (data[i].real != 0)
    	  data[i].real = atan2(data[i].imag, data[i].real);
	else
    	  data[i].real = 0;
#ifdef MATH_CHECK_FPE
	if (!finite(data[i].real) || !finite(data[i].imag))
	    ma_fpe_errno = FLT_OTHER;
#endif
    }
    [MathArray _finishMath];
    return [self maReal];
}

- maConjugate
{
    unsigned long i, max_elements;
    MATH_TYPE *data;

    [MathArray _startMath];
    [self _updateLazyArray];
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData  mutableBytes];
    for (i=0; i < max_elements; i++) {
#if MATH_ORDER == 1
	complex_double num;
	num = cconj(complexf2d(data, i));
    	data[i] = complexd2f(&num, 0);
#else
    	data[i] = cconj(data[i]);
#endif
    }
    [MathArray _finishMath];
    return self;
}

/* Performs a Fast-Fourier Transform on the receiver in the specified 
   direction: -1 for a forward transform, +1 for an inverse transform. */
- maFFt:(int)direction
{
    int i;
    const unsigned  *sizes;
    MATH_TYPE *data;

    [self _updateLazyArray];
    sizes = [size bytes];
    /* FIXME: Currently FFTs are restricted to arrays of powers of
       two */
    for (i=0; i < dimension; i++)
    	if (sizes[i] != ((int)pow(2, log(sizes[i])/log(2))))
		[NSException raise:MAPerformException
		    format:@"Cannot perform FFT on non power-of-two arrays."];
    data = [arrayData mutableBytes];
    [MathArray _startMath];
    /* FIXME: Expect complex numbers to just be two reals in a row. OK? */
    fourn((MATH_REAL_TYPE *)data, sizes, dimension, direction);
    [MathArray _finishMath];
    
    /* Scale forward transform by array size */
    if (direction < 0)
    	[self maDivide:[MANumber 
		numberWithDouble:array_num_elements(dimension, sizes)]];
    return self;
}

- maInvert
{
    [self maNotImplemented:_cmd];
    return self;
}

- (id <NSNumber,ComplexNumber>)_loopArrayWith:
	(complex_double (*)(complex_double, complex_double))loopFunc
{
    unsigned long i, max_elements;
    const MATH_TYPE *data;
    complex_double number;
		
    [MathArray _startMath];
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData  bytes];
#if MATH_ORDER == 1
    number = complexf2d(data, 0);
#else
    number = data[0];
#endif
    for (i=1; i < max_elements; i++)
#if MATH_ORDER == 1
        number = loopFunc(number, complexf2d(data, i));
#else
        number = loopFunc(number, data[i]);
#endif
    [MathArray _finishMath];
	    
    return (MANumber *)[MANumber value:&number withObjCType:@encode(complex_double)];
}

- (id <NSNumber,ComplexNumber>)maMinimumValue
{
    return [self _loopArrayWith:find_cmin];
}

- (id <NSNumber,ComplexNumber>)maMaximumValue
{
    return [self _loopArrayWith:find_cmax];
}

- (id <NSNumber,ComplexNumber>)maTotal
{
    return [self _loopArrayWith:find_csum];
}

- maWhere
{
    ordered_index_t i, max_elements;
    unsigned j;
    MATH_TYPE *data;

    j = 0;
    max_elements = array_num_elements(dimension, [size bytes]);
    data = [arrayData mutableBytes];
    for (i=0; i < max_elements; i++)
        if (data[i].real || data[i].imag)
	    data[j++].real = i;
    
    [self maReal];
    [arrayData setCount:j];
    [size release];
    dimension = 1;
    size = [[MAValueData dataWithValues:&j
    		count:dimension
		objCType:@encode(unsigned)] retain];
    return self;
}

@end

/* All the functions which know how to cast from any known type to our type */
static inline MATH_TYPE
cast_from_uchar(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((unsigned char*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_char(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((char*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_ushort(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((unsigned short*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_short(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((short*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_uint(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((unsigned int*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_int(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((int*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_ulong(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((unsigned long*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_long(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((long*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_ulonglong(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((unsigned long long*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_longlong(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((long long*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_float(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((float*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_double(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  *((double*)data + loc);
    t.imag = 0;
    return t;
}

static inline MATH_TYPE
cast_from_complexfloat(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  ((complex_float*)data + loc)->real;
    t.imag =  ((complex_float*)data + loc)->imag;
    return t;
}

static inline MATH_TYPE
cast_from_complexdouble(void *data, unsigned long loc) 
{
    MATH_TYPE t;
    t.real =  ((complex_double*)data + loc)->real;
    t.imag =  ((complex_double*)data + loc)->imag;
    return t;
}


static caster_f_t
cast_function(const char *type)
{
  switch(*type) {
  case _C_CHR:
    return cast_from_char;
    break;
  case _C_UCHR:
    return cast_from_uchar;
    break;
  case _C_SHT:
    return cast_from_short;
    break;
  case _C_USHT:
    return cast_from_ushort;
    break;
  case _C_INT:
    return cast_from_int;
    break;
  case _C_UINT:
    return cast_from_uint;
    break;
  case _C_LNG:
    return cast_from_long;
    break;
  case _C_ULNG:
    return cast_from_ulong;
    break;
  case 'q':
    return cast_from_longlong;
    break;
  case 'Q':
    return cast_from_ulonglong;
    break;
  case _C_FLT:
    return cast_from_float;
    break;
  case _C_DBL:
    return cast_from_double;
    break;
  case _C_STRUCT_B:
      // FIXME: need better check for complex
      if (math_aligned_size (type) 
      		== math_aligned_size (@encode(complex_float)))
	return cast_from_complexfloat;
      else if (math_aligned_size (type) 
      		== math_aligned_size (@encode(complex_double)))
	return cast_from_complexdouble;
    break;
  }

  MA_RAISE_CAST;
  /* NOT REACHED */
  return 0;
}

/*-------------------------------------------------------------------fourn

  fourn - N-dimensional fourier transform from Numerical Recipies

    data  - ndim-dimensional array of complex numbers stored in row major order
    nn    - an integer array containing the lengths of each dimension
    ndim  - number of dimensions of the array data
    isign - direction of F.T (-1 for foreward transform)

    Extra special sneaky addition:  If abs(isign) > 1, then this routine
    also prints out a percentage indication of how far along the routine
    is in processing (isign is also set back to isign/abs(isign)).
*/

#define SWAP(a,b) (tempr=(a),(a)=(b),(b)=tempr)

static void 
fourn(MATH_REAL_TYPE *data, const unsigned *nn, int ndim, int isign)
{
  int i1,i2,i3,i2rev,i3rev,ip1,ip2,ip3,ifp1,ifp2;
  int ibit,idim,k1,k2,n,nprev,nrem,ntot;
  int count, total;
  double tempi,tempr;
  double theta,wi,wpi,wpr,wr,wtemp;

  ntot=1;
  total=0;
  for (idim=0;idim<ndim;idim++)
    ntot *= nn[idim];

  /* Extra special sneaky addition -- If abs(isign) is greater than one, this
     is a signal to print out our completion status
  */
  if (abs(isign) > 1) {
    isign = (isign > 0) ? 1 : -1;
    total = 0;
    count = ntot;
    while ((count = count >> 1))
	total++;
  }

  nprev=1;
  for (idim=ndim-1;idim>=0;idim--) {
    n=nn[idim];
    nrem=ntot/(n*nprev);
    ip1=nprev << 1;
    ip2=ip1*n;
    ip3=ip2*nrem;
    i2rev=1;
    for (i2=1;i2<=ip2;i2+=ip1) {
      if (i2 < i2rev) {
	for (i1=i2;i1<=i2+ip1-2;i1+=2) {
	  for (i3=i1;i3<=ip3;i3+=ip2) {
	    i3rev=i2rev+i3-i2;
	    SWAP(data[i3-1],data[i3rev-1]);
	    SWAP(data[i3  ],data[i3rev  ]);
	  }
	}
      }
      ibit=ip2 >> 1;
      while (ibit >= ip1 && i2rev > ibit) {
	i2rev -= ibit;
	ibit >>= 1;
      }
      i2rev += ibit;
    }
    ifp1=ip1;
    while (ifp1 < ip2) {
#if defined(NEXT_FOUNDATION) || OBJECTS_MINOR_VERSION != 1
      if (total)
	[[NSNotificationCenter defaultCenter] 
          postNotificationName: STATUS_CHECK_NOTIFICATION
          object: [NSNumber numberWithInt: ((float)(++count)/total)*100]];
#endif
      ifp2=ifp1 << 1;
      theta=isign*6.28318530717959/(ifp2/ip1);
      wtemp=sin(0.5*theta);
      wpr = -2.0*wtemp*wtemp;
      wpi=sin(theta);
      wr=1.0;
      wi=0.0;
      for (i3=1;i3<=ifp1;i3+=ip1) {
	for (i1=i3;i1<=i3+ip1-2;i1+=2) {
	  for (i2=i1;i2<=ip3;i2+=ifp2) {
	    k1=i2;
	    k2=k1+ifp1;
	    tempr=wr*data[k2-1]-wi*data[k2];
	    tempi=wr*data[k2]+wi*data[k2-1];
	    data[k2-1]=data[k1-1]-tempr;
	    data[k2]=data[k1]-tempi;
	    data[k1-1] += tempr;
	    data[k1  ] += tempi;
	  }
	}
	wr=(wtemp=wr)*wpr-wi*wpi+wr;
	wi=wi*wpr+wtemp*wpi+wi;
      }
      ifp1=ifp2;
    }
    nprev *= n;
  }
}


