/*

 *      GPAC - Multimedia Framework C SDK

 *

 *      Authors: Jean Le Feuvre

 *      Copyright (c) Telecom ParisTech 2000-2012

 *          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 Lesser 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 Lesser General Public License for more details.

 *

 *  You should have received a copy of the GNU Lesser General Public

 *  License along with this library; see the file COPYING.  If not, write to

 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.

 *

 */

#include "math.h"

#include "quant.h"

#ifndef GPAC_DISABLE_BIFS_ENC

GF_Err gf_bifs_enc_qp_set(GF_BifsEncoder *codec, GF_Node *qp)

{

  if (gf_node_get_tag(qp) != TAG_MPEG4_QuantizationParameter) return GF_BAD_PARAM;

  /*if we have an active QP, push it into the stack*/

  if (codec->ActiveQP && ((GF_Node*)codec->ActiveQP != codec->scene_graph->global_qp) )

    gf_list_insert(codec->QPs, codec->ActiveQP, 0);

  codec->ActiveQP = (M_QuantizationParameter *)qp;

  return GF_OK;

}

GF_Err gf_bifs_enc_qp_remove(GF_BifsEncoder *codec, Bool ActivatePrev)

{

  codec->ActiveQP = NULL;

  if (!ActivatePrev) return GF_OK;

  if (gf_list_count(codec->QPs)) {

    codec->ActiveQP = (M_QuantizationParameter*)gf_list_get(codec->QPs, 0);

    gf_list_rem(codec->QPs, 0);

  } else if (codec->scene_graph->global_qp) {

    codec->ActiveQP = (M_QuantizationParameter *)codec->scene_graph->global_qp;

  }

  return GF_OK;

}

u32 gf_bifs_enc_qp14_get_bits(GF_BifsEncoder *codec)

{

  if (!codec->ActiveQP || !codec->coord_stored) return 0;

  return (u32) ceil(log1p(codec->NumCoord) / log(2) );

}

void gf_bifs_enc_qp14_enter(GF_BifsEncoder *codec, Bool Enter)

{

  if (!codec->ActiveQP) return;

  if (Enter) codec->storing_coord = GF_TRUE;

  else {

    if (codec->storing_coord) codec->coord_stored = GF_TRUE;

    codec->storing_coord = GF_FALSE;

  }

}

void gf_bifs_enc_qp14_reset(GF_BifsEncoder *codec)

{

  codec->coord_stored = GF_FALSE;

  codec->storing_coord = GF_FALSE;

  codec->NumCoord = 0;

}

void gf_bifs_enc_qp14_set_length(GF_BifsEncoder *codec, u32 NbElements)

{

  if (!codec->ActiveQP || !codec->storing_coord || codec->coord_stored) return;

  codec->NumCoord = NbElements;

}

void gf_bifs_enc_mantissa_float(GF_BifsEncoder *codec, Fixed ft, GF_BitStream *bs)

{

  u32 mantLength, expLength, mantSign, mantissa, expSign, i, nbBits;

  s32 exp;

  union

  {

    Float f;

    s32 l;

  } ft_val;

  if (ft == 0) {

    gf_bs_write_int(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;

  gf_bs_write_int(bs, nbBits+1, 4);

  if (mantLength) {

    gf_bs_write_int(bs, expLength, 3);

    gf_bs_write_int(bs, mantSign, 1);

    gf_bs_write_int(bs, mantissa, nbBits);

    if(expLength) {

      gf_bs_write_int(bs, expSign, 1);

      gf_bs_write_int(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(gf_floor( FLT2FIX(_v+0.5) ) );

}

GF_Err Q_EncFloat(GF_BifsEncoder *codec, GF_BitStream *bs, u32 FieldType, SFVec3f BMin, SFVec3f BMax, u32 NbBits, void *field_ptr)

{

  s32 newVal;

  switch (FieldType) {

  case GF_SG_VRML_SFINT32:

    return GF_NON_COMPLIANT_BITSTREAM;

  case GF_SG_VRML_SFFLOAT:

    newVal = Q_Quantize(BMin.x, BMax.x, NbBits, *((SFFloat *)field_ptr));

    gf_bs_write_int(bs, newVal, NbBits);

    return GF_OK;

  case GF_SG_VRML_SFVEC2F:

    newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFVec2f *)field_ptr)->x);

    gf_bs_write_int(bs, newVal, NbBits);

    newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFVec2f *)field_ptr)->y);

    gf_bs_write_int(bs, newVal, NbBits);

    return GF_OK;

  case GF_SG_VRML_SFVEC3F:

    newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFVec3f *)field_ptr)->x);

    gf_bs_write_int(bs, newVal, NbBits);

    newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFVec3f *)field_ptr)->y);

    gf_bs_write_int(bs, newVal, NbBits);

    newVal = Q_Quantize(BMin.z, BMax.z, NbBits, ((SFVec3f *)field_ptr)->z);

    gf_bs_write_int(bs, newVal, NbBits);

    return GF_OK;

  case GF_SG_VRML_SFCOLOR:

    newVal = Q_Quantize(BMin.x, BMax.x, NbBits, ((SFColor *)field_ptr)->red);

    gf_bs_write_int(bs, newVal, NbBits);

    newVal = Q_Quantize(BMin.y, BMax.y, NbBits, ((SFColor *)field_ptr)->green);

    gf_bs_write_int(bs, newVal, NbBits);

    newVal = Q_Quantize(BMin.z, BMax.z, NbBits, ((SFColor *)field_ptr)->blue);

    gf_bs_write_int(bs, newVal, NbBits);

    return GF_OK;

  case GF_SG_VRML_SFROTATION:

    //forbidden in this Q mode

    return GF_NON_COMPLIANT_BITSTREAM;

  }

  return GF_OK;

}

//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

GF_Err Q_EncInt(GF_BifsEncoder *codec, GF_BitStream *bs, u32 QType, SFInt32 b_min, u32 NbBits, void *field_ptr)

{

  switch (QType) {

  case QC_LINEAR_SCALAR:

  case QC_COORD_INDEX:

    gf_bs_write_int(bs, *((SFInt32 *)field_ptr) - b_min, NbBits);

    return GF_OK;

  default:

    return GF_NON_COMPLIANT_BITSTREAM;

  }

}

GF_Err Q_EncCoordOnUnitSphere(GF_BifsEncoder *codec, GF_BitStream *bs, u32 NbBits, u32 NbComp, Fixed *m_ft)

{

  u32 i;

  u32 len = NbComp+1;

  u32 orientation=0;

  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) gf_bs_write_int(bs, ((m_ft[orientation]>0) ? 0 : 1), 1);

  gf_bs_write_int(bs, orientation, 2);

  for (i=0; i<NbComp; i++) {

    Fixed v = gf_mulfix(gf_divfix(INT2FIX(4), GF_PI) , gf_atan2(m_ft[orientation], m_ft[(orientation+i+1) % len]));

    s32 qdt = Q_Quantize(0, 1, NbBits-1, (v>=0 ? v : -v));

    s32 qv = (1<<(NbBits-1)) + (v>=0 ? 1 : -1) * qdt;

    gf_bs_write_int(bs, qv, NbBits);

  }

  return GF_OK;

}

GF_Err Q_EncNormal(GF_BifsEncoder *codec, GF_BitStream *bs, u32 NbBits, void *field_ptr)

{

  Fixed comp[3];

  SFVec3f v =  * (SFVec3f *)field_ptr;

  gf_vec_norm(&v);

  comp[0] = v.x;

  comp[1] = v.y;

  comp[2] = v.z;

  return Q_EncCoordOnUnitSphere(codec, bs, NbBits, 2, comp);

}

GF_Err Q_EncRotation(GF_BifsEncoder *codec, GF_BitStream *bs, u32 NbBits, void *field_ptr)

{

  GF_Vec4 quat;

  Fixed comp[4];

  /*get quaternion*/

  quat = gf_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);

}

GF_Err gf_bifs_enc_quant_field(GF_BifsEncoder *codec, GF_BitStream *bs, GF_Node *node, GF_FieldInfo *field)

{

  Bool HasQ;

  u8 QType, AType;

  u32 NbBits;

  Fixed b_min, b_max;

  SFVec3f BMin, BMax;

  GF_Err e;

  /*check QP*/

  if (!codec->ActiveQP) return GF_EOS;

  /*check FieldType*/

  switch (field->fieldType) {

  case GF_SG_VRML_SFINT32:

  case GF_SG_VRML_SFFLOAT:

  case GF_SG_VRML_SFROTATION:

  case GF_SG_VRML_SFVEC2F:

  case GF_SG_VRML_SFVEC3F:

    break;

  case GF_SG_VRML_SFCOLOR:

    break;

  default:

    return GF_EOS;

  }

  /*check NDT*/

  HasQ = gf_bifs_get_aq_info(node, field->fieldIndex, &QType, &AType, &b_min, &b_max, &NbBits);

  if (!HasQ || !QType) return GF_EOS;

  /*get NbBits for QP14 (QC_COORD_INDEX)*/

  if (QType == QC_COORD_INDEX) {

    NbBits = gf_bifs_enc_qp14_get_bits(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 GF_NON_COMPLIANT_BITSTREAM;

  }

  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 GF_EOS;

  /*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 != GF_SG_VRML_SFVEC3F) return GF_NON_COMPLIANT_BITSTREAM;

    e = Q_EncNormal(codec, bs, NbBits, field->far_ptr);

    break;

  case QC_ROTATION:

    //normal quant is only for SFVec3F

    if (field->fieldType != GF_SG_VRML_SFROTATION) return GF_NON_COMPLIANT_BITSTREAM;

    e = Q_EncRotation(codec, bs, NbBits, field->far_ptr);

    break;

  default:

    return GF_BAD_PARAM;

  }

  return e;

}

#endif  /*GPAC_DISABLE_BIFS_ENC*/