/*
 * Copyright (C) 2008 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/* ---- includes ----------------------------------------------------------- */

#include "b_TensorEm/Flt16Mat2D.h"
#include "b_TensorEm/Functions.h"
#include "b_BasicEm/Math.h"

/* ------------------------------------------------------------------------- */

/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ auxiliary functions } ---------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */

/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ constructor / destructor } ----------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */

void bts_Flt16Mat2D_init( struct bts_Flt16Mat2D* ptrA )
{
	ptrA->bbpE = 0;
	ptrA->xxE = 0;
	ptrA->xyE = 0;
	ptrA->yxE = 0;
	ptrA->yyE = 0;
}

/* ------------------------------------------------------------------------- */

void bts_Flt16Mat2D_exit( struct bts_Flt16Mat2D* ptrA )
{
	ptrA->bbpE = 0;
	ptrA->xxE = 0;
	ptrA->xyE = 0;
	ptrA->yxE = 0;
	ptrA->yyE = 0;
}

/* ------------------------------------------------------------------------- */

/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ operators } -------------------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */

void bts_Flt16Mat2D_copy( struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
{
	ptrA->bbpE = srcPtrA->bbpE;
	ptrA->xxE = srcPtrA->xxE;
	ptrA->xyE = srcPtrA->xyE;
	ptrA->yxE = srcPtrA->yxE;
	ptrA->yyE = srcPtrA->yyE;
}

/* ------------------------------------------------------------------------- */

flag bts_Flt16Mat2D_equal( const struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
{
	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
	if( bbpDiffL == 0 )
	{
		if( ptrA->xxE != srcPtrA->xxE ) return FALSE;
		if( ptrA->xyE != srcPtrA->xyE ) return FALSE;
		if( ptrA->yxE != srcPtrA->yxE ) return FALSE;
		if( ptrA->yyE != srcPtrA->yyE ) return FALSE;
		return TRUE;
	}

	if( bbpDiffL > 0 )
	{
		int32 xxL = ( int32 ) srcPtrA->xxE << bbpDiffL;
		int32 xyL = ( int32 ) srcPtrA->xyE << bbpDiffL;
		int32 yxL = ( int32 ) srcPtrA->yxE << bbpDiffL;
		int32 yyL = ( int32 ) srcPtrA->yyE << bbpDiffL;

		if( ptrA->xxE != xxL ) return FALSE;
		if( ptrA->xyE != xyL ) return FALSE;
		if( ptrA->yxE != yxL ) return FALSE;
		if( ptrA->yyE != yyL ) return FALSE;

		/* check if bits were lost by the shifting */
		if( srcPtrA->xxE != ( xxL >> bbpDiffL ) ) return FALSE;
		if( srcPtrA->xyE != ( xyL >> bbpDiffL ) ) return FALSE;
		if( srcPtrA->yxE != ( yxL >> bbpDiffL ) ) return FALSE;
		if( srcPtrA->yyE != ( yyL >> bbpDiffL ) ) return FALSE;

		return TRUE;
	}

	if( bbpDiffL < 0 )
	{
		int32 xxL = ( int32 ) ptrA->xxE << -bbpDiffL;
		int32 xyL = ( int32 ) ptrA->xyE << -bbpDiffL;
		int32 yxL = ( int32 ) ptrA->yxE << -bbpDiffL;
		int32 yyL = ( int32 ) ptrA->yyE << -bbpDiffL;

		if( xxL != srcPtrA->xxE ) return FALSE;
		if( xyL != srcPtrA->xyE ) return FALSE;
		if( yxL != srcPtrA->yxE ) return FALSE;
		if( yyL != srcPtrA->yyE ) return FALSE;

		/* check if bits were lost by the shifting */
		if( ptrA->xxE != ( xxL >> -bbpDiffL ) ) return FALSE;
		if( ptrA->xyE != ( xyL >> -bbpDiffL ) ) return FALSE;
		if( ptrA->yxE != ( yxL >> -bbpDiffL ) ) return FALSE;
		if( ptrA->yyE != ( yyL >> -bbpDiffL ) ) return FALSE;

		return TRUE;
	}

	return TRUE;
}

/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ query functions } -------------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */

/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ modify functions } ------------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */
	
/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ I/O } -------------------------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */
	
/* ========================================================================= */
/*                                                                           */
/* ---- \ghd{ exec functions } --------------------------------------------- */
/*                                                                           */
/* ========================================================================= */

/* ------------------------------------------------------------------------- */

uint32 bts_Flt16Mat2D_det( const struct bts_Flt16Mat2D* ptrA )
{
	/* This could be negativ, in theory. But almost always det > 0 for us,
	   matrix is a rotation or scaling matrix.
	   Then uint32 makes sure there is no overflow. */
	uint32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
	return detL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_createIdentity()
{
	struct bts_Flt16Mat2D matL = { 1 << 14, 0, 0, 1 << 14, 14 };
	return matL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_createRotation( phase16 angleA )
{
	int16 cL = bbs_cos16( angleA );
	int16 sL = bbs_sin16( angleA );
	struct bts_Flt16Mat2D matL;
	matL.xxE =  cL;
	matL.xyE = -sL;
	matL.yxE =  sL;
	matL.yyE =  cL;
	matL.bbpE = 14;
	return matL; 
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_createScale( int32 scaleA, int32 scaleBbpA )
{
	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createIdentity();
	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
	return matL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_createRigid( phase16 angleA, int32 scaleA, int32 scaleBbpA )
{
	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createRotation( angleA );
	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
	return matL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_create16( int16 xxA, int16 xyA, int16 yxA, int16 yyA, int16 bbpA )
{
	struct bts_Flt16Mat2D matL;
	matL.xxE = xxA;
	matL.xyE = xyA;
	matL.yxE = yxA;
	matL.yyE = yyA;
	matL.bbpE = bbpA;
	return matL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_create32( int32 xxA, int32 xyA, int32 yxA, int32 yyA, int32 bbpA )
{
	struct bts_Flt16Mat2D matL;

	if( ( xxA | xyA | yxA | yyA ) == 0 )
	{
		matL.xxE = 0;
		matL.xyE = 0;
		matL.yxE = 0;
		matL.yyE = 0;
		matL.bbpE = 0;
	}
	else
	{
		int32 shiftL = bts_maxAbsIntLog2Of4( xxA, xyA, yxA, yyA ) - 13;

		if( shiftL > 0 )
		{
			int32 sh1L = shiftL - 1;
			matL.xxE = ( ( xxA >> sh1L ) + 1 ) >> 1;
			matL.xyE = ( ( xyA >> sh1L ) + 1 ) >> 1;
			matL.yxE = ( ( yxA >> sh1L ) + 1 ) >> 1;
			matL.yyE = ( ( yyA >> sh1L ) + 1 ) >> 1;
		}
		else
		{
			matL.xxE = xxA << -shiftL;
			matL.xyE = xyA << -shiftL;
			matL.yxE = yxA << -shiftL;
			matL.yyE = yyA << -shiftL;
		}

		matL.bbpE = bbpA - shiftL;
	}

	return matL;
}

/* ------------------------------------------------------------------------- */

void bts_Flt16Mat2D_scale( struct bts_Flt16Mat2D* ptrA, int32 scaleA, int32 scaleBbpA )
{
	/* fit scale in 15 bit */
	uint32 scaleExpL = bts_absIntLog2( scaleA );
	if( scaleExpL > 14 )
	{
		int32 shiftL = scaleExpL - 14;
		scaleA = ( ( scaleA >> ( shiftL - 1 ) ) + 1 ) >> 1;
		scaleBbpA -= shiftL;
	}
	
	*ptrA = bts_Flt16Mat2D_create32( (int32)ptrA->xxE * scaleA,
									 (int32)ptrA->xyE * scaleA,
									 (int32)ptrA->yxE * scaleA,
									 (int32)ptrA->yyE * scaleA,
									 ptrA->bbpE + scaleBbpA );
}

/* ------------------------------------------------------------------------- */

struct bts_Int16Vec2D bts_Flt16Mat2D_map( const struct bts_Flt16Mat2D* matPtrA, 
								          const struct bts_Int16Vec2D* vecPtrA )
{
	struct bts_Int16Vec2D vecL;

	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;

	if( matPtrA->bbpE > 0 )
	{
		int32 sh1L = matPtrA->bbpE - 1;
		vecL.xE = ( ( xL >> sh1L ) + 1 ) >> 1;
		vecL.yE = ( ( yL >> sh1L ) + 1 ) >> 1;
	}
	else
	{
		/* not overflow safe */
		vecL.xE = xL << -matPtrA->bbpE;
		vecL.yE = yL << -matPtrA->bbpE;
	}

	return vecL;
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Vec2D bts_Flt16Mat2D_mapFlt( const struct bts_Flt16Mat2D* matPtrA, 
								             const struct bts_Flt16Vec2D* vecPtrA )
{
	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;
	int32 bbpL = matPtrA->bbpE + vecPtrA->bbpE;
	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_mul( const struct bts_Flt16Mat2D* mat1PtrA, 
								          const struct bts_Flt16Mat2D* mat2PtrA )
{
	return bts_Flt16Mat2D_create32( ( int32 ) mat1PtrA->xxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yxE,
									( int32 ) mat1PtrA->xxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yyE,
									( int32 ) mat1PtrA->yxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yxE,
									( int32 ) mat1PtrA->yxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yyE,
									mat1PtrA->bbpE + mat2PtrA->bbpE );
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D* bts_Flt16Mat2D_mulTo( struct bts_Flt16Mat2D* mat1PtrA, 
				                             const struct bts_Flt16Mat2D* mat2PtrA )
{
	*mat1PtrA = bts_Flt16Mat2D_mul( mat1PtrA, mat2PtrA );
	return mat1PtrA;
}

/* ------------------------------------------------------------------------- */

void bts_Flt16Mat2D_invert( struct bts_Flt16Mat2D* ptrA )
{
	int32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
	int32 detExpL = bbs_intLog2( detL );
	int32 dShrL = 0;
	if( detExpL > 15 )
	{
		dShrL = detExpL - 15; 
		detL = ( ( detL >> ( dShrL - 1 ) ) + 1 ) >> 1;
	}

	if( detL == 0 )
	{
		ptrA->xxE = ptrA->yyE = ptrA->xyE = ptrA->yxE = 0;
	}
	else
	{
		/* bbp: bbpE + 16 - ( bbpE * 2 - dShrL ) = 16 + dShrL - bbpE */
		int32 xxL = ( ( int32 )ptrA->xxE << 16 ) / detL;
		int32 xyL = ( ( int32 )ptrA->xyE << 16 ) / detL;
		int32 yxL = ( ( int32 )ptrA->yxE << 16 ) / detL;
		int32 yyL = ( ( int32 )ptrA->yyE << 16 ) / detL;
		*ptrA = bts_Flt16Mat2D_create32( xxL, -xyL, -yxL, yyL, 16 + dShrL - ptrA->bbpE );
	}
}

/* ------------------------------------------------------------------------- */

struct bts_Flt16Mat2D bts_Flt16Mat2D_inverted( const struct bts_Flt16Mat2D* ptrA )
{
	struct bts_Flt16Mat2D matL = *ptrA;
	bts_Flt16Mat2D_invert( &matL );
	return matL;
}

/* ------------------------------------------------------------------------- */

/* ========================================================================= */