/*

** Copyright (C) 2003-2011 Erik de Castro Lopo <erikd@mega-nerd.com>

**

** This program 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.1 of the License, or

** (at your option) any later version.

**

** This program 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 program; if not, write to the Free Software

** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

*/

#include  "sfconfig.h"

#include  <stdlib.h>

#include  "sndfile.h"

#include  "sfendian.h"

#include  "common.h"

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

**  Rule number 1 is to only apply dither when going from a larger bitwidth

**  to a smaller bitwidth. This can happen on both read and write.

**

**  Need to apply dither on all conversions marked X below.

**

**  Dither on write:

**

**                    Input

**          | short   int     float   double

**      --------+-----------------------------------------------

**    O 8 bit | X     X     X     X

**    u 16 bit  | none    X     X     X

**    t 24 bit  | none    X     X     X

**    p 32 bit  | none    none    X     X

**    u float | none    none    none    none

**    t double  | none    none    none    none

**

**  Dither on read:

**

**                    Input

**    O     | 8 bit 16 bit  24 bit  32 bit  float double

**    u --------+-------------------------------------------------

**    t short | none  none  X   X   X   X

**    p int   | none  none  none  X   X   X

**    u float | none  none  none  none  none  none

**    t double  | none  none  none  none  none  none

*/

#define SFE_DITHER_BAD_PTR  666

#define SFE_DITHER_BAD_TYPE 667

typedef struct

{ int     read_short_dither_bits, read_int_dither_bits ;

  int     write_short_dither_bits, write_int_dither_bits ;

  double    read_float_dither_scale, read_double_dither_bits ;

  double    write_float_dither_scale, write_double_dither_bits ;

  sf_count_t  (*read_short) (SF_PRIVATE *psf, short *ptr, sf_count_t len) ;

  sf_count_t  (*read_int)   (SF_PRIVATE *psf, int *ptr, sf_count_t len) ;

  sf_count_t  (*read_float) (SF_PRIVATE *psf, float *ptr, sf_count_t len) ;

  sf_count_t  (*read_double)  (SF_PRIVATE *psf, double *ptr, sf_count_t len) ;

  sf_count_t  (*write_short)  (SF_PRIVATE *psf, const short *ptr, sf_count_t len) ;

  sf_count_t  (*write_int)  (SF_PRIVATE *psf, const int *ptr, sf_count_t len) ;

  sf_count_t  (*write_float)  (SF_PRIVATE *psf, const float *ptr, sf_count_t len) ;

  sf_count_t  (*write_double) (SF_PRIVATE *psf, const double *ptr, sf_count_t len) ;

  double buffer [SF_BUFFER_LEN / sizeof (double)] ;

} DITHER_DATA ;

static sf_count_t dither_read_short   (SF_PRIVATE *psf, short *ptr, sf_count_t len) ;

static sf_count_t dither_read_int   (SF_PRIVATE *psf, int *ptr, sf_count_t len) ;

static sf_count_t dither_write_short  (SF_PRIVATE *psf, const short *ptr, sf_count_t len) ;

static sf_count_t dither_write_int    (SF_PRIVATE *psf, const int *ptr, sf_count_t len) ;

static sf_count_t dither_write_float  (SF_PRIVATE *psf, const float *ptr, sf_count_t len) ;

static sf_count_t dither_write_double (SF_PRIVATE *psf, const double *ptr, sf_count_t len) ;

int

dither_init (SF_PRIVATE *psf, int mode)

{ DITHER_DATA *pdither ;

  pdither = psf->dither ; /* This may be NULL. */

  /* Turn off dither on read. */

  if (mode == SFM_READ && psf->read_dither.type == SFD_NO_DITHER)

  { if (pdither == NULL)

      return 0 ; /* Dither is already off, so just return. */

    if (pdither->read_short)

      psf->read_short = pdither->read_short ;

    if (pdither->read_int)

      psf->read_int = pdither->read_int ;

    if (pdither->read_float)

      psf->read_float = pdither->read_float ;

    if (pdither->read_double)

      psf->read_double = pdither->read_double ;

    return 0 ;

    } ;

  /* Turn off dither on write. */

  if (mode == SFM_WRITE && psf->write_dither.type == SFD_NO_DITHER)

  { if (pdither == NULL)

      return 0 ; /* Dither is already off, so just return. */

    if (pdither->write_short)

      psf->write_short = pdither->write_short ;

    if (pdither->write_int)

      psf->write_int = pdither->write_int ;

    if (pdither->write_float)

      psf->write_float = pdither->write_float ;

    if (pdither->write_double)

      psf->write_double = pdither->write_double ;

    return 0 ;

    } ;

  /* Turn on dither on read if asked. */

  if (mode == SFM_READ && psf->read_dither.type != 0)

  { if (pdither == NULL)

      pdither = psf->dither = calloc (1, sizeof (DITHER_DATA)) ;

    if (pdither == NULL)

      return SFE_MALLOC_FAILED ;

    switch (SF_CODEC (psf->sf.format))

    { case SF_FORMAT_DOUBLE :

      case SF_FORMAT_FLOAT :

          pdither->read_int = psf->read_int ;

          psf->read_int = dither_read_int ;

          break ;

      case SF_FORMAT_PCM_32 :

      case SF_FORMAT_PCM_24 :

      case SF_FORMAT_PCM_16 :

      case SF_FORMAT_PCM_S8 :

      case SF_FORMAT_PCM_U8 :

          pdither->read_short = psf->read_short ;

          psf->read_short = dither_read_short ;

          break ;

      default : break ;

      } ;

    } ;

  /* Turn on dither on write if asked. */

  if (mode == SFM_WRITE && psf->write_dither.type != 0)

  { if (pdither == NULL)

      pdither = psf->dither = calloc (1, sizeof (DITHER_DATA)) ;

    if (pdither == NULL)

      return SFE_MALLOC_FAILED ;

    switch (SF_CODEC (psf->sf.format))

    { case SF_FORMAT_DOUBLE :

      case SF_FORMAT_FLOAT :

          pdither->write_int = psf->write_int ;

          psf->write_int = dither_write_int ;

          break ;

      case SF_FORMAT_PCM_32 :

      case SF_FORMAT_PCM_24 :

      case SF_FORMAT_PCM_16 :

      case SF_FORMAT_PCM_S8 :

      case SF_FORMAT_PCM_U8 :

          break ;

      default : break ;

      } ;

    pdither->write_short = psf->write_short ;

    psf->write_short = dither_write_short ;

    pdither->write_int = psf->write_int ;

    psf->write_int = dither_write_int ;

    pdither->write_float = psf->write_float ;

    psf->write_float = dither_write_float ;

    pdither->write_double = psf->write_double ;

    psf->write_double = dither_write_double ;

    } ;

  return 0 ;

} /* dither_init */

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

*/

static void dither_short  (const short *in, short *out, int frames, int channels) ;

static void dither_int    (const int *in, int *out, int frames, int channels) ;

static void dither_float  (const float *in, float *out, int frames, int channels) ;

static void dither_double (const double *in, double *out, int frames, int channels) ;

static sf_count_t

dither_read_short (SF_PRIVATE * UNUSED (psf), short * UNUSED (ptr), sf_count_t len)

{

  return len ;

} /* dither_read_short */

static sf_count_t

dither_read_int (SF_PRIVATE * UNUSED (psf), int * UNUSED (ptr), sf_count_t len)

{

  return len ;

} /* dither_read_int */

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

*/

static sf_count_t

dither_write_short  (SF_PRIVATE *psf, const short *ptr, sf_count_t len)

{ DITHER_DATA *pdither ;

  int     bufferlen, writecount, thiswrite ;

  sf_count_t  total = 0 ;

  if ((pdither = psf->dither) == NULL)

  { psf->error = SFE_DITHER_BAD_PTR ;

    return 0 ;

    } ;

  switch (SF_CODEC (psf->sf.format))

  { case SF_FORMAT_PCM_S8 :

    case SF_FORMAT_PCM_U8 :

    case SF_FORMAT_DPCM_8 :

        break ;

    default :

      return pdither->write_short (psf, ptr, len) ;

    } ;

  bufferlen = sizeof (pdither->buffer) / (sizeof (short)) ;

  while (len > 0)

  { writecount = (len >= bufferlen) ? bufferlen : (int) len ;

    writecount /= psf->sf.channels ;

    writecount *= psf->sf.channels ;

    dither_short (ptr, (short*) pdither->buffer, writecount / psf->sf.channels, psf->sf.channels) ;

    thiswrite = (int) pdither->write_short (psf, (short*) pdither->buffer, writecount) ;

    total += thiswrite ;

    len -= thiswrite ;

    if (thiswrite < writecount)

      break ;

    } ;

  return total ;

} /* dither_write_short */

static sf_count_t

dither_write_int  (SF_PRIVATE *psf, const int *ptr, sf_count_t len)

{ DITHER_DATA *pdither ;

  int     bufferlen, writecount, thiswrite ;

  sf_count_t  total = 0 ;

  if ((pdither = psf->dither) == NULL)

  { psf->error = SFE_DITHER_BAD_PTR ;

    return 0 ;

    } ;

  switch (SF_CODEC (psf->sf.format))

  { case SF_FORMAT_PCM_S8 :

    case SF_FORMAT_PCM_U8 :

    case SF_FORMAT_PCM_16 :

    case SF_FORMAT_PCM_24 :

        break ;

    case SF_FORMAT_DPCM_8 :

    case SF_FORMAT_DPCM_16 :

        break ;

    default :

      return pdither->write_int (psf, ptr, len) ;

    } ;

  bufferlen = sizeof (pdither->buffer) / (sizeof (int)) ;

  while (len > 0)

  { writecount = (len >= bufferlen) ? bufferlen : (int) len ;

    writecount /= psf->sf.channels ;

    writecount *= psf->sf.channels ;

    dither_int (ptr, (int*) pdither->buffer, writecount / psf->sf.channels, psf->sf.channels) ;

    thiswrite = (int) pdither->write_int (psf, (int*) pdither->buffer, writecount) ;

    total += thiswrite ;

    len -= thiswrite ;

    if (thiswrite < writecount)

      break ;

    } ;

  return total ;

} /* dither_write_int */

static sf_count_t

dither_write_float  (SF_PRIVATE *psf, const float *ptr, sf_count_t len)

{ DITHER_DATA *pdither ;

  int     bufferlen, writecount, thiswrite ;

  sf_count_t  total = 0 ;

  if ((pdither = psf->dither) == NULL)

  { psf->error = SFE_DITHER_BAD_PTR ;

    return 0 ;

    } ;

  switch (SF_CODEC (psf->sf.format))

  { case SF_FORMAT_PCM_S8 :

    case SF_FORMAT_PCM_U8 :

    case SF_FORMAT_PCM_16 :

    case SF_FORMAT_PCM_24 :

        break ;

    case SF_FORMAT_DPCM_8 :

    case SF_FORMAT_DPCM_16 :

        break ;

    default :

      return pdither->write_float (psf, ptr, len) ;

    } ;

  bufferlen = sizeof (pdither->buffer) / (sizeof (float)) ;

  while (len > 0)

  { writecount = (len >= bufferlen) ? bufferlen : (int) len ;

    writecount /= psf->sf.channels ;

    writecount *= psf->sf.channels ;

    dither_float (ptr, (float*) pdither->buffer, writecount / psf->sf.channels, psf->sf.channels) ;

    thiswrite = (int) pdither->write_float (psf, (float*) pdither->buffer, writecount) ;

    total += thiswrite ;

    len -= thiswrite ;

    if (thiswrite < writecount)

      break ;

    } ;

  return total ;

} /* dither_write_float */

static sf_count_t

dither_write_double (SF_PRIVATE *psf, const double *ptr, sf_count_t len)

{ DITHER_DATA *pdither ;

  int     bufferlen, writecount, thiswrite ;

  sf_count_t  total = 0 ;

  if ((pdither = psf->dither) == NULL)

  { psf->error = SFE_DITHER_BAD_PTR ;

    return 0 ;

    } ;

  switch (SF_CODEC (psf->sf.format))

  { case SF_FORMAT_PCM_S8 :

    case SF_FORMAT_PCM_U8 :

    case SF_FORMAT_PCM_16 :

    case SF_FORMAT_PCM_24 :

        break ;

    case SF_FORMAT_DPCM_8 :

    case SF_FORMAT_DPCM_16 :

        break ;

    default :

      return pdither->write_double (psf, ptr, len) ;

    } ;

  bufferlen = sizeof (pdither->buffer) / sizeof (double) ;

  while (len > 0)

  { writecount = (len >= bufferlen) ? bufferlen : (int) len ;

    writecount /= psf->sf.channels ;

    writecount *= psf->sf.channels ;

    dither_double (ptr, (double*) pdither->buffer, writecount / psf->sf.channels, psf->sf.channels) ;

    thiswrite = (int) pdither->write_double (psf, (double*) pdither->buffer, writecount) ;

    total += thiswrite ;

    len -= thiswrite ;

    if (thiswrite < writecount)

      break ;

    } ;

  return total ;

} /* dither_write_double */

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

*/

static void

dither_short (const short *in, short *out, int frames, int channels)

{ int ch, k ;

  for (ch = 0 ; ch < channels ; ch++)

    for (k = ch ; k < channels * frames ; k += channels)

      out [k] = in [k] ;

} /* dither_short */

static void

dither_int (const int *in, int *out, int frames, int channels)

{ int ch, k ;

  for (ch = 0 ; ch < channels ; ch++)

    for (k = ch ; k < channels * frames ; k += channels)

      out [k] = in [k] ;

} /* dither_int */

static void

dither_float (const float *in, float *out, int frames, int channels)

{ int ch, k ;

  for (ch = 0 ; ch < channels ; ch++)

    for (k = ch ; k < channels * frames ; k += channels)

      out [k] = in [k] ;

} /* dither_float */

static void

dither_double (const double *in, double *out, int frames, int channels)

{ int ch, k ;

  for (ch = 0 ; ch < channels ; ch++)

    for (k = ch ; k < channels * frames ; k += channels)

      out [k] = in [k] ;

} /* dither_double */

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

*/

#if 0

/*

** Not made public because this (maybe) requires storage of state information.

**

** Also maybe need separate state info for each channel!!!!

*/

int

DO_NOT_USE_sf_dither_short (const SF_DITHER_INFO *dither, const short *in, short *out, int frames, int channels)

{ int ch, k ;

  if (! dither)

    return SFE_DITHER_BAD_PTR ;

  switch (dither->type & SFD_TYPEMASK)

  { case SFD_WHITE :

    case SFD_TRIANGULAR_PDF :

        for (ch = 0 ; ch < channels ; ch++)

          for (k = ch ; k < channels * frames ; k += channels)

            out [k] = in [k] ;

        break ;

    default :

      return SFE_DITHER_BAD_TYPE ;

    } ;

  return 0 ;

} /* DO_NOT_USE_sf_dither_short */

int

DO_NOT_USE_sf_dither_int (const SF_DITHER_INFO *dither, const int *in, int *out, int frames, int channels)

{ int ch, k ;

  if (! dither)

    return SFE_DITHER_BAD_PTR ;

  switch (dither->type & SFD_TYPEMASK)

  { case SFD_WHITE :

    case SFD_TRIANGULAR_PDF :

        for (ch = 0 ; ch < channels ; ch++)

          for (k = ch ; k < channels * frames ; k += channels)

            out [k] = in [k] ;

        break ;

    default :

      return SFE_DITHER_BAD_TYPE ;

    } ;

  return 0 ;

} /* DO_NOT_USE_sf_dither_int */

int

DO_NOT_USE_sf_dither_float (const SF_DITHER_INFO *dither, const float *in, float *out, int frames, int channels)

{ int ch, k ;

  if (! dither)

    return SFE_DITHER_BAD_PTR ;

  switch (dither->type & SFD_TYPEMASK)

  { case SFD_WHITE :

    case SFD_TRIANGULAR_PDF :

        for (ch = 0 ; ch < channels ; ch++)

          for (k = ch ; k < channels * frames ; k += channels)

            out [k] = in [k] ;

        break ;

    default :

      return SFE_DITHER_BAD_TYPE ;

    } ;

  return 0 ;

} /* DO_NOT_USE_sf_dither_float */

int

DO_NOT_USE_sf_dither_double (const SF_DITHER_INFO *dither, const double *in, double *out, int frames, int channels)

{ int ch, k ;

  if (! dither)

    return SFE_DITHER_BAD_PTR ;

  switch (dither->type & SFD_TYPEMASK)

  { case SFD_WHITE :

    case SFD_TRIANGULAR_PDF :

        for (ch = 0 ; ch < channels ; ch++)

          for (k = ch ; k < channels * frames ; k += channels)

            out [k] = in [k] ;

        break ;

    default :

      return SFE_DITHER_BAD_TYPE ;

    } ;

  return 0 ;

} /* DO_NOT_USE_sf_dither_double */

#endif