/*
 *			GPAC - MPEG-4 Systems C Development Kit
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *					All rights reserved
 *
 *  This file is part of GPAC / BIFS codec sub-project
 *
 *  GPAC is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2, or (at your option)
 *  any later version.
 *   
 *  GPAC is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *   
 *  You should have received a copy of the GNU General Public License
 *  along with GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */

#include "Quantize.h"


M4Err BE_RegisterQP(BifsEncoder *codec, SFNode *qp)
{
	if (Node_GetTag(qp) != TAG_MPEG4_QuantizationParameter) return M4BadParam;

	/*if we have an active QP, push it into the stack*/
	if (codec->ActiveQP && (codec->ActiveQP != codec->GlobalQP) ) 
		ChainInsertEntry(codec->QPs, codec->ActiveQP, 0);
	
	codec->ActiveQP = (M_QuantizationParameter *)qp;
	return M4OK;
}

M4Err BE_RemoveQP(BifsEncoder *codec, Bool ActivatePrev)
{
	codec->ActiveQP = NULL;
	if (!ActivatePrev) return M4OK;

	if (ChainGetCount(codec->QPs)) {
		codec->ActiveQP = ChainGetEntry(codec->QPs, 0);
		ChainDeleteEntry(codec->QPs, 0);
	} else if (codec->GlobalQP) {
		codec->ActiveQP = codec->GlobalQP;
	}
	return M4OK;
}


u32 BE_GetCoordBits(BifsEncoder *codec)
{
	if (!codec->ActiveQP || !codec->coord_stored) return 0;
	return (u32) ceil(log(codec->NumCoord+1) / log(2) ); 
}

void BE_EnterCoord(BifsEncoder *codec, Bool Enter)
{
	if (!codec->ActiveQP) return;
	if (Enter) codec->storing_coord = 1;
	else {
		if (codec->storing_coord) codec->coord_stored = 1;
		codec->storing_coord = 0;
	}
}

void BE_ResetQP14(BifsEncoder *codec)
{
	codec->coord_stored = 0;
	codec->storing_coord = 0;
	codec->NumCoord = 0;
}

void BE_SetCoordLength(BifsEncoder *codec, u32 NbElements)
{
	if (!codec->ActiveQP || !codec->storing_coord || codec->coord_stored) return;
	codec->NumCoord = NbElements;
}

void BE_WriteMantissaFloat(BifsEncoder *codec, Fixed ft, BitStream *bs)
{
	u32 mantLength, expLength, mantSign, mantissa, expSign, i, nbBits;
	s32 exp;

	union
	{	
		Float f;
		s32 l;
	} ft_val;

	if (ft == 0) {
	    BS_WriteInt(bs, 0, 4);
		return;
	}
	ft_val.f = FIX2FLT(ft);
  
	mantSign = ((ft_val.l & 0x80000000) >> 31) & 0x1;
	mantissa = (ft_val.l & 0x007FFFFF) >> 9;
	mantLength = 15;
	expSign=0;
	exp =(((ft_val.l & 0x7F800000) >> 23)-127);
	expLength = 8;
  
	if (mantissa == 0) mantLength = 1;
  

	if (exp) {
		if (exp< 0) {
			expSign = 1;
			exp = -exp;	    
		}
		while ((exp & (1<<(--expLength)))==0) { }
		exp &= ~(1<<expLength);
		expLength++;
	} else {
		expLength=0;
	}
  
	nbBits=0;
	for(i = mantissa; i>0; ++nbBits) i >>= 1;

	BS_WriteInt(bs, nbBits+1, 4);
	if (mantLength) {
		BS_WriteInt(bs, expLength, 3);
		BS_WriteInt(bs, mantSign, 1);
		BS_WriteInt(bs, mantissa, nbBits);
		if(expLength) {
			BS_WriteInt(bs, expSign, 1);
			BS_WriteInt(bs, exp, expLength - 1);
		}
	}
}


//Linear Quantization for floats - go back to float to avoid overflow if nbBits more than 15...
s32 Q_Quantize(Fixed Min, Fixed Max, u32 NbBits, Fixed value)
{
	Float _v;
	if (value <= Min) return 0;
	if (value >= Max) return (1<<NbBits)-1;
	_v = FIX2FLT(value - Min);
	_v *= (1 << NbBits) - 1;
	_v /= FIX2FLT(Max - Min);
	return FIX2INT(m4_floor( FLT2FIX(_v+0.5) ) );
}


M4Err Q_EncFloat(BifsEncoder *codec, BitStream *bs, u32 FieldType, SFVec3f BMin, SFVec3f BMax, u32 NbBits, void *field_ptr)
{
	s32 newVal;
	switch (FieldType) {
	case FT_SFInt32:
		return M4InvalidNode;
	case FT_SFFloat:
		newVal = Q_Quantize(BMin.x, BMax.x, NbBits, *((SFFloat *)field_ptr));
		BS_WriteInt(bs, newVal, NbBits);
		return M4OK;
	case FT_SFVec2f:
		newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFVec2f *)field_ptr)->x);
		BS_WriteInt(bs, newVal, NbBits);
		newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFVec2f *)field_ptr)->y);
		BS_WriteInt(bs, newVal, NbBits);
		return M4OK;
	case FT_SFVec3f:
		newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFVec3f *)field_ptr)->x);
		BS_WriteInt(bs, newVal, NbBits);
		newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFVec3f *)field_ptr)->y);
		BS_WriteInt(bs, newVal, NbBits);
		newVal = Q_Quantize(BMin.z, BMax.z, NbBits, ((SFVec3f *)field_ptr)->z);
		BS_WriteInt(bs, newVal, NbBits);
		return M4OK;
	case FT_SFColor:
		newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFColor *)field_ptr)->red);
		BS_WriteInt(bs, newVal, NbBits);
		newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFColor *)field_ptr)->green);
		BS_WriteInt(bs, newVal, NbBits);
		newVal = Q_Quantize(BMin.z, BMax.z, NbBits, ((SFColor *)field_ptr)->blue);
		BS_WriteInt(bs, newVal, NbBits);
		return M4OK;

	case FT_SFRotation:
		//forbidden in this Q mode
		return M4InvalidNode;
	}
	return M4OK;
}

//int in quant are either Linear Scalar fields or CoordIndex
//the quant is just a bitshifting into [0, 2^NbBits-1]
//so v = value - b_min
M4Err Q_EncInt(BifsEncoder *codec, BitStream *bs, u32 QType, SFInt32 b_min, u32 NbBits, void *field_ptr)
{
	switch (QType) {
	case QC_LINEAR_SCALAR:
	case QC_COORD_INDEX:
		BS_WriteInt(bs, *((SFInt32 *)field_ptr) - b_min, NbBits);
		return M4OK;
	default:
		return M4NonCompliantBitStream;
	}
}

M4Err Q_EncCoordOnUnitSphere(BifsEncoder *codec, BitStream *bs, u32 NbBits, u32 NbComp, Fixed *m_ft) 
{
	u32 i;
	u32 len = NbComp+1;
	s32 orientation =-1;
	Fixed maxTmp = - FIX_MAX;
	for (i=0; i<len; i++) {
		if (ABS(m_ft[i]) > maxTmp) {
			maxTmp = ABS(m_ft[i]);
			orientation = i;
		}
	}
	if(NbComp==2) BS_WriteInt(bs, ((m_ft[orientation]>0) ? 0 : 1), 1); 
	BS_WriteInt(bs, orientation, 2);
	for (i=0; i<NbComp; i++) {
		Fixed v = m4_mulfix(m4_divfix(INT2FIX(4), M4_PI) , m4_atan2(m_ft[(orientation+i+1) % len], m_ft[orientation]));
		s32 qdt = Q_Quantize(0, 1, NbBits-1, (v>=0 ? v : -v));
		s32 qv = (1<<(NbBits-1)) + (v>=0 ? 1 : -1) * qdt;
		BS_WriteInt(bs, qv, NbBits);
	}
	return M4OK;
}

M4Err Q_EncNormal(BifsEncoder *codec, BitStream *bs, u32 NbBits, void *field_ptr)
{
	Fixed comp[3];
	SFVec3f v =  * (SFVec3f *)field_ptr;
	m4_vec_norm(&v);
	comp[0] = v.x;
	comp[1] = v.y;
	comp[2] = v.z;
	return Q_EncCoordOnUnitSphere(codec, bs, NbBits, 2, comp);
}

M4Err Q_EncRotation(BifsEncoder *codec, BitStream *bs, u32 NbBits, void *field_ptr)
{
	M4Vec4 quat;
	Fixed comp[4];
	
	/*get quaternion*/
	quat = m4_quat_from_rotation(*(SFRotation *)field_ptr);
	comp[0] = quat.q;
	comp[1] = quat.x;
	comp[2] = quat.y;
	comp[3] = quat.z;
	return Q_EncCoordOnUnitSphere(codec, bs, NbBits, 3, comp);
}

M4Err BE_QuantizeField(BifsEncoder *codec, BitStream *bs, SFNode *node, FieldInfo *field)
{
	Bool HasQ;
	u8 QType, AType;
	u32 NbBits;
	Fixed b_min, b_max;
	SFVec3f BMin, BMax;
	M4Err e;

	/*check QP*/
	if (!codec->ActiveQP) return M4FieldNotQuantized;
	/*check FieldType*/
	switch (field->fieldType) {
	case FT_SFInt32:
	case FT_SFFloat:
	case FT_SFRotation:
	case FT_SFVec2f:
	case FT_SFVec3f:
		break;
	case FT_SFColor:
		break;
	default:
		return M4FieldNotQuantized;
	}
	
	/*check NDT*/
	HasQ = Node_GetAQInfo(node, field->fieldIndex, &QType, &AType, &b_min, &b_max, &NbBits);
	if (!HasQ || !QType) return M4FieldNotQuantized;

	/*get NbBits for QP14 (QC_COORD_INDEX)*/
	if (QType == QC_COORD_INDEX) {
		NbBits = BE_GetCoordBits(codec);
		/*QP14 is always on, not having NbBits set means the coord field is set after the index field, hence not decodable*/
		if (!NbBits) 
			return M4NonCompliantBitStream;
	}

	BMin.x = BMin.y = BMin.z = b_min;
	BMax.x = BMax.y = BMax.z = b_max;

	/*check is the QP is on and retrieves the bounds*/
	if (!Q_IsTypeOn(codec->ActiveQP, QType, &NbBits, &BMin, &BMax)) return M4FieldNotQuantized;

	/*ok the field is Quantized, dequantize*/
	switch (QType) {
	//these are all SFFloat quantized on n fields
	case QC_3DPOS:
	case QC_2DPOS:
	case QC_ORDER:
	case QC_COLOR:
	case QC_TEXTURE_COORD:
	case QC_ANGLE:
	case QC_SCALE:
	case QC_INTERPOL_KEYS:
	case QC_SIZE_3D:
	case QC_SIZE_2D:
		e = Q_EncFloat(codec, bs, field->fieldType, BMin, BMax, NbBits, field->far_ptr);
		break;
	//SFInt types
	case QC_LINEAR_SCALAR:
	case QC_COORD_INDEX:
		e = Q_EncInt(codec, bs, QType, (SFInt32) b_min, NbBits, field->far_ptr);
		break;
	//normalized fields (normals and vectors)
	case QC_NORMALS:
		//normal quant is only for SFVec3F
		if (field->fieldType != FT_SFVec3f) return M4InvalidNode;
		e = Q_EncNormal(codec, bs, NbBits, field->far_ptr);
		break;
	case QC_ROTATION:
		//normal quant is only for SFVec3F
		if (field->fieldType != FT_SFRotation) return M4InvalidNode;
		e = Q_EncRotation(codec, bs, NbBits, field->far_ptr);
		break;
	default:
		return M4BadParam;
	}
	return e;
}
