/**
	\file miscAsm.s
	
*/
#include <vs1005h.h>
#include <vstypes.h>
#include "../miscAsm.h"


#ifdef ASM
/*
void Matrix3To2(register __i0 s_int16 *s, register __i2 s_int16 *d,
		register __i3 s_int16 __mem_y *coeff,
		register __a0 s_int16 samples);
*/
	.sect code,Matrix3To2
	.export _Matrix3To2
_Matrix3To2:
	add a0,ones,a0;	ldx (i6)+1,null
	stx a1,(I6)+1;	sty i1,(I6)
	stx le,(I6)+1;	sty ls,(I6)
	stx d1,(I6)+1;	sty lc,(I6)
	stx b0,(I6)+1;	sty b1,(I6)
	stx c0,(I6)+1;	sty c1,(I6)
	jns $9
	and d,null,d;	stx d0,(I6)+1
	mv i2,i7	// i7 = d
	ldc 2,i2
	ldc 1,i1
	stx d0,(i6);	sty d1,(i6)	// Clear to of stack

	ldc 16-10,b0
	ldx (i0)*,a1;	ldy (i3)*,c1
	loop a0,$8-1
	mulss a1,c1;	ldx (i0)*,a0;	ldy (i3)*,c0
	
	macss a0,c0,d;	ldx (i0),a0;	ldy (i3),c0
	macss a0,c0,d;	ldy (i0)-2,null
	add d,p,d;	ldy (i3)-3,null
	ashl d,b0,d;	ldx (i0)*,a1;	ldy (i3)*,c1
	rnd d,b1;	ldx (i6),d0;	ldy (i6),d1
	stx b1,(i7)++
	
	mulss a1,c1;	ldx (i0)*,a0;	ldy (i3)*,c0
	macss a0,c0,d;	ldx (i0)*,a0;	ldy (i3),c0
	macss a0,c0,d; ldy (i3)-5,null
	add d,p,d;	ldx (i0)*,a1;	ldy (i3)*,c1
	ashl d,b0,d;	ldx (i0)*,a0;	ldy (i3)*,c0
	rnd d,b1;	ldx (i6),d0;	ldy (i6),d1
	mulss a1,c1;	stx b1,(i7)++

$8:
$9:
	ldx (I6)-1,null
	ldx (I6)-1,d0
	ldx (I6)-1,c0;	ldy (I6),c1
	ldx (I6)-1,b0;	ldy (I6),b1
	ldx (I6)-1,d1;	ldy (I6),lc
	ldx (I6)-1,le;	ldy (I6),ls
	jr
	ldx (I6)-1,a1;	ldy (I6),i1



/*
void Matrix3To2L(register __i0 s_int32 *s, register __i2 s_int32 *d,
		 register __i3 s_int16 __mem_y *coeff,
		 register __a0 s_int16 samples);
*/
	.sect code,Matrix3To2L
	.export _Matrix3To2L
_Matrix3To2L:
	add a0,ones,a0;	ldx (i6)+1,null
	stx i0,(I6)+1;	sty i1,(I6)
	stx le,(I6)+1;	sty ls,(I6)
	stx a1,(I6)+1;	sty lc,(I6)
	stx b0,(I6)+1;	sty b1,(I6)
	stx c0,(I6)+1;	sty c1,(I6)
	stx d0,(I6)+1;	sty d1,(I6)
	jns $9
	mv i2,i7	// i7 = d

	ldc -10,c1
	ldc 6,d1
	ldc 2,i2
	ldc 1,i1
	stx d2,(i6);	sty d2,(i6)	// d1=6 -> d2=0

	ldx (i0)*,d0;	ldy (i3)*,c0

	loop a0,$8-1
	and a,null,a;	ldx (i6),b0;	ldy (i6),b1

	mulus d0,c0;	ldx (i0)*,d0
	macss d0,c0,a;	ldx (i0)*,d0;	ldy (i3)*,c0
	macus d0,c0,b;	ldx (i0)*,d0
	macss d0,c0,a;	ldx (i0)*,d0;	ldy (i3),c0
	macus d0,c0,b;	ldx (i0)-5,d0
	macss d0,c0,a;	ldy (i3)-3,null
	add b,p,b;	ldx (i0)*,d0;	ldy (i3)*,c0

	ashl a,c1,a
	ashl b,d1,b
	add a,b,b;	ldx (i6),a0;	ldy (i6),a1
	sat b,b
	stx b0,(i7)++
	and b,null,b;	stx b1,(i7)++

	mulus d0,c0;	ldx (i0)*,d0
	macss d0,c0,a;	ldx (i0)*,d0;	ldy (i3)*,c0
	macus d0,c0,b;	ldx (i0)*,d0
	macss d0,c0,a;	ldx (i0)*,d0;	ldy (i3),c0
	macus d0,c0,b;	ldx (i0)*,d0
	macss d0,c0,a;	ldy (i3)-5,null
	add b,p,b;	ldx (i0)*,d0;	ldy (i3)*,c0

	ashl a,c1,a
	ashl b,d1,b
	add a,b,b;	ldx (i6),a0;	ldy (i6),a1
	sat b,b
	stx b0,(i7)++
	and b,null,b;	stx b1,(i7)++
	
$8:
$9:
	ldx (I6)-1,null
	ldx (I6)-1,d0;	ldy (I6),d1
	ldx (I6)-1,c0;	ldy (I6),c1
	ldx (I6)-1,b0;	ldy (I6),b1
	ldx (I6)-1,a1;	ldy (I6),lc
	ldx (I6)-1,le;	ldy (I6),ls
	jr
	ldx (I6)-1,i0;	ldy (I6),i1



#if 0
    /* Below is a non-optimized version of Matrix3To2L(), as an example
       of how to potentially expand to other matrix sizes. */

/*
void Matrix3To2L_Unoptimized(register __i0 s_int32 *s, register __i2 s_int32 *d,
		 register __i3 s_int16 __mem_y *coeff,
		 register __a0 s_int16 samples);
*/
	.sect code,Matrix3To2L_Unoptimized
	.export _Matrix3To2L_Unoptimized
_Matrix3To2L_Unoptimized:
	add a0,ones,a0;	ldx (i6)+1,null
	stx i0,(I6)+1;	sty i1,(I6)
	stx i2,(I6)+1;	sty i3,(I6)
	stx le,(I6)+1;	sty ls,(I6)
	stx lr0,(I6)+1;	sty lc,(I6)
	stx a0,(I6)+1;	sty a1,(I6)
	stx b0,(I6)+1;	sty b1,(I6)
	stx c0,(I6)+1;	sty c1,(I6)
	stx d0,(I6)+1;	sty d1,(I6)
	stx i4,(I6)

	jns $9
	mv i2,i7	// i7 = d

	loop a0,$8-1
	nop

	and a,null,a
	and b,null,b
	
	ldx (i0)+1,d0;	ldy (i3)+2,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)+1,d0
	mulss d0,c0
	add b,p,b

	ldx (i0)+1,d0;	ldy (i3)+2,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)+1,d0
	mulss d0,c0
	add b,p,b

	ldx (i0)+1,d0;	ldy (i3)-3,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)-5,d0
	mulss d0,c0
	add b,p,b

	ldc -10,c1
	ashl a,c1,a
	ldc 6,c1
	ashl b,c1,b
	add a,b,a
	sat a,a
	stx a0,(i7)++
	stx a1,(i7)++

	and a,null,a
	and b,null,b
	
	ldx (i0)+1,d0;	ldy (i3)+2,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)+1,d0
	mulss d0,c0
	add b,p,b

	ldx (i0)+1,d0;	ldy (i3)+2,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)+1,d0
	mulss d0,c0
	add b,p,b

	ldx (i0)+1,d0;	ldy (i3)-5,c0
	mulus d0,c0
	add a,p,a
	ldx (i0)+1,d0
	mulss d0,c0
	add b,p,b

	ldc -10,c1
	ashl a,c1,a
	ldc 6,c1
	ashl b,c1,b
	add a,b,a
	sat a,a
	stx a0,(i7)++
	stx a1,(i7)++
$8:
$9:
	ldx (I6)-1,i4
	ldx (I6)-1,d0;	ldy (I6),d1
	ldx (I6)-1,c0;	ldy (I6),c1
	ldx (I6)-1,b0;	ldy (I6),b1
	ldx (I6)-1,a0;	ldy (I6),a1
	ldx (I6)-1,lr0;	ldy (I6),lc
	ldx (I6)-1,le;	ldy (I6),ls
	ldx (I6)-1,i2;	ldy (I6),i3
	jr
	ldx (I6)-1,i0;	ldy (I6),i1

	/* End of non-optimized version of Matrix3To2L() */

#endif


	.end



#if 0
	/********* EVERYTHING FROM NOW ON IS PURELY DECORATIONAL!!! **********/

/*
	void Generic(void) - 2018 edition
*/
	.sect code,Generic
	.export _Generic
_Generic:
	ldx (i6)+1,null
	stx i0,(I6)+1;	sty i1,(I6)
	stx i2,(I6)+1;	sty i3,(I6)
	stx le,(I6)+1;	sty ls,(I6)
	stx lr0,(I6)+1;	sty lc,(I6)
	stx a0,(I6)+1;	sty a1,(I6)
	stx b0,(I6)+1;	sty b1,(I6)
	stx c0,(I6)+1;	sty c1,(I6)
	stx d0,(I6)+1;	sty d1,(I6)
	stx i4,(I6)

	ldx (I6)-1,i4
	ldx (I6)-1,d0;	ldy (I6),d1
	ldx (I6)-1,c0;	ldy (I6),c1
	ldx (I6)-1,b0;	ldy (I6),b1
	ldx (I6)-1,a0;	ldy (I6),a1
	ldx (I6)-1,lr0;	ldy (I6),lc
	ldx (I6)-1,le;	ldy (I6),ls
	ldx (I6)-1,i2;	ldy (I6),i3
	jr
	ldx (I6)-1,i0;	ldy (I6),i1
#endif
