#ifndef __CAST_H__
#define __CAST_H__

#include <gctypes.h>

#define	GQR2			914
#define	GQR3			915
#define	GQR4			916
#define	GQR5			917
#define	GQR6			918
#define	GQR7			919

#define GQR_TYPE_F32	0
#define GQR_TYPE_U8		4
#define GQR_TYPE_U16	5
#define GQR_TYPE_S8		6
#define GQR_TYPE_S16	7

#define GQR_CAST_U8		2
#define GQR_CAST_U16	3
#define GQR_CAST_S8		4
#define GQR_CAST_S16	5

#ifdef __cplusplus
extern "C" {
#endif

#ifdef GEKKO

#define __set_gqr(_reg,_val)	__asm__ __volatile__ ("mtspr %0,%1" : : "i"(_reg), "b"(_val))

// does a default init
static inline void CAST_Init(void)
{
	__asm__ __volatile__ (
		"li		3,0x0004\n\
		 oris	3,3,0x0004\n\
		 mtspr	914,3\n\
		 li		3,0x0005\n\
		 oris	3,3,0x0005\n\
		 mtspr	915,3\n\
		 li		3,0x0006\n\
		 oris	3,3,0x0006\n\
		 mtspr	916,3\n\
		 li		3,0x0007\n\
		 oris	3,3,0x0007\n\
		 mtspr	917,3\n"
		 : : : "r3"
	);
}

#define GQR_SCALE_MASK (0x3f << 8)

static inline void CAST_SetGQR2(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR2,val);
}

static inline void CAST_SetGQR3(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR3,val);
}

static inline void CAST_SetGQR4(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR4,val);
}

static inline void CAST_SetGQR5(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR5,val);
}

static inline void CAST_SetGQR6(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR6,val);
}

static inline void CAST_SetGQR7(u32 type,s32 scale)
{
	 u32 val = ((((((scale)<<8)&GQR_SCALE_MASK)|(type))<<16)|((((scale)<<8)&GQR_SCALE_MASK)|(type)));
	__set_gqr(GQR7,val);
}


/******************************************************************/
/*																  */
/* cast from int to float										  */
/*																  */
/******************************************************************/

static inline f32 __castu8f32( u8 *in)
{
	 f32 rval;
	__asm__ __volatile__ (
		"psq_l	%[rval],0(%[in]),1,2" : [rval]"=f"(rval) : [in]"r"(in)
	);
	return rval;
}

static inline f32 __castu16f32( u16 *in)
{
	 f32 rval;
	__asm__ __volatile__ (
		"psq_l	%[rval],0(%[in]),1,3" : [rval]"=f"(rval) : [in]"r"(in)
	);
	return rval;
}

static inline f32 __casts8f32( s8 *in)
{
	 f32 rval;
	__asm__ __volatile__ (
		"psq_l	%[rval],0(%[in]),1,4" : [rval]"=f"(rval) : [in]"r"(in)
	);
	return rval;
}

static inline f32 __casts16f32( s16 *in)
{
	 f32 rval;
	__asm__ __volatile__ (
		"psq_l	%[rval],0(%[in]),1,5" : [rval]"=f"(rval) : [in]"r"(in)
	);
	return rval;
}

static inline void castu8f32( u8 *in, volatile f32 *out)
{
	*out = __castu8f32(in);
}

static inline void castu16f32( u16 *in, volatile f32 *out)
{
	*out = __castu16f32(in);
}

static inline void casts8f32( s8 *in, volatile f32 *out)
{
	*out = __casts8f32(in);
}

static inline void casts16f32( s16 *in, volatile f32 *out)
{
	*out = __casts16f32(in);
}

/******************************************************************/
/*																  */
/* cast from float to int										  */
/*																  */
/******************************************************************/

static inline u8 __castf32u8( f32 in)
{
	f32 a;
	 u8 rval;
	 f32 *ptr = &a;

	__asm__ __volatile__ (
		"psq_st	%[in],0(%[ptr]),1,2\n"
		"lbz	%[out],0(%[ptr])\n"
		: [out]"=r"(rval), [ptr]"+r"(ptr) : [in]"f"(in)
	);
	return rval;
}

static inline u16 __castf32u16( f32 in)
{
	f32 a;
	 u16 rval;
	 f32 *ptr = &a;

	__asm__ __volatile__ (
		"psq_st	%[in],0(%[ptr]),1,3\n"
		"lhz	%[out],0(%[ptr])\n"
		: [out]"=r"(rval), [ptr]"+r"(ptr) : [in]"f"(in)
	);
	return rval;
}

static inline s8 __castf32s8( f32 in)
{
	f32 a;
	 s8 rval;
	 f32 *ptr = &a;

	__asm__ __volatile__ (
		"psq_st	%[in],0(%[ptr]),1,4\n"
		"lbz	%[out],0(%[ptr])\n"
		: [out]"=r"(rval), [ptr]"+r"(ptr) : [in]"f"(in)
	);
	return rval;
}

static inline s16 __castf32s16( f32 in)
{
	f32 a;
	 s16 rval;
	 f32 *ptr = &a;

	__asm__ __volatile__ (
		"psq_st	%[in],0(%[ptr]),1,5\n"
		"lha	%[out],0(%[ptr])\n"
		: [out]"=r"(rval), [ptr]"+r"(ptr) : [in]"f"(in)
	);
	return rval;
}

static inline void castf32u8( f32 *in, vu8 *out)
{
	*out = __castf32u8(*in);
}

static inline void castf32u16( f32 *in, vu16 *out)
{
	*out = __castf32u16(*in);
}

static inline void castf32s8( f32 *in, vs8 *out)
{
	*out = __castf32s8(*in);
}

static inline void castf32s16( f32 *in, vs16 *out)
{
	*out = __castf32s16(*in);
}

#endif //GEKKO

#ifdef __cplusplus
}
#endif

#endif
