#include <vo_stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vstypes.h>
#include <saturate.h>
#include "../miscAsm.h"

#ifdef __VSDSP__
void Matrix3To2(register __i0 s_int16 *s, register __i2 s_int16 *d,
		register __i3 s_int16 __mem_y *coeff,
		register __a0 s_int16 samples);
#else
/*
  NOTE!
  This C code doesn't have all necessary overflow code in place, nor
  does it have rounding for the 16-bit result. It may overflow with
  really big coeff values.
  The C code is just a template for the more thorough assembler
  implementation in miscAsm.s.
 */
void Matrix3To2(register __i0 s_int16 *s, register __i2 s_int16 *d,
		register __i3 s_int16 __mem_y *coeff,
		register __a0 s_int16 samples) {
  int i;
  for (i=0; i<samples; i++) {
    s_int32 t32;

    t32 = (s_int32)*s * *coeff;
    s += 1; coeff += 2;
    t32 += (s_int32)*s * *coeff;
    s += 1; coeff += 2;
    t32 += (s_int32)*s * *coeff;
    s -= 2; coeff -= 3;
    *d++ = Sat32To16(t32>>10);

    t32 = (s_int32)*s * *coeff;
    s += 1; coeff += 2;
    t32 += (s_int32)*s * *coeff;
    s += 1; coeff += 2;
    t32 += (s_int32)*s * *coeff;
    s += 1; coeff -= 5;
    *d++ = Sat32To16(t32>>10);
  }
}
#endif


#ifdef __VSDSP_
void Matrix3To2L(register __i0 s_int32 *s, register __i2 s_int32 *d,
		 register __i3 s_int16 __mem_y *coeff,
		 register __a0 s_int16 samples);
#else
/*
  NOTE!
  This C code doesn't have all necessary overflow code in place.
  It may overflow if the absolute value of the sum for the coefficients
  for an output channel is over 1.
  The C code is just a template for the more thorough assembler
  implementation in miscAsm.s.
 */
void Matrix3To2L(register __i0 s_int32 *s, register __i2 s_int32 *d,
		 register __i3 s_int16 __mem_y *coeff,
		 register __a0 s_int16 samples) {
  s_int32 *origS=s, *origD=d;
  s_int16 origSamples=samples;
  s_int16 *s16=s, *d16=d;
  int i;

  memset(d, 0, 4*samples);
  for (i=0; i<samples; i++) {
    s_int32 t32L, t32H;
    t32L = (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H = (s_int32)*s16 * *coeff;
    s16 += 1; coeff += 2;
    t32L += (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H += (s_int32)*s16 * *coeff;
    s16 += 1; coeff += 2;
    t32L += (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H += (s_int32)*s16 * *coeff;
    s16 -= 5; coeff -= 3;
    t32H = (t32H << 6) | (t32L >> 10);
    *d16++ = (s_int16)t32H;
    *d16++ = (s_int16)(t32H>>16);

    t32L = (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H = (s_int32)*s16 * *coeff;
    s16 += 1; coeff += 2;
    t32L += (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H += (s_int32)*s16 * *coeff;
    s16 += 1; coeff += 2;
    t32L += (s_int32)((u_int16)*s16) * *coeff;
    s16 += 1;
    t32H += (s_int32)*s16 * *coeff;
    s16 += 1; coeff -= 5;
    t32H = (t32H << 6) + (t32L >> 10);
    *d16++ = (s_int16)t32H;
    *d16++ = (s_int16)(t32H>>16);
  }
  
#if 0
  printf("M3To2L %d\n", origSamples);
  for (i=0; i<origSamples; i++) {
    printf("%8ld %8ld %8ld -> %8ld %8ld\n",
	   origS[3*i+0], origS[3*i+1], origS[3*i+2],
	   origD[2*i+0], origD[2*i+1]);
  }
  Delay(100);
#endif
}
#endif
