// Game_Music_Emu https://bitbucket.org/mpyne/game-music-emu/

#include "Sap_Apu.h"

#include <string.h>

/* Copyright (C) 2006 Shay Green. This module 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

module 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 module; if not, write to the Free Software Foundation,

Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */

#include "blargg_source.h"

static int const max_frequency = 12000; // pure waves above this frequency are silenced

static void gen_poly( uint32_t mask, int count, byte* out )

{

  uint32_t n = 1;

  do

  {

    int bits = 0;

    int b = 0;

    do

    {

      // implemented using "Galios configuration"

      bits |= (n & 1) << b;

      n = (n >> 1) ^ (mask & uMinus(n & 1));

    }

    while ( b++ < 7 );

    *out++ = bits;

  }

  while ( --count );

}

// poly5

static int const poly5_len = (1 <<  5) - 1;

static uint32_t const poly5_mask = (1UL << poly5_len) - 1;

static uint32_t const poly5 = 0x167C6EA1;

static inline uint32_t run_poly5( uint32_t in, int shift )

{

  return (in << shift & poly5_mask) | (in >> (poly5_len - shift));

}

#define POLY_MASK( width, tap1, tap2 ) \

  ((1UL << (width - 1 - tap1)) | (1UL << (width - 1 - tap2)))

Sap_Apu_Impl::Sap_Apu_Impl()

{

  gen_poly( POLY_MASK(  4, 1, 0 ), sizeof poly4,  poly4  );

  gen_poly( POLY_MASK(  9, 5, 0 ), sizeof poly9,  poly9  );

  gen_poly( POLY_MASK( 17, 5, 0 ), sizeof poly17, poly17 );

  if ( 0 ) // comment out to recauculate poly5 constant

  {

    byte poly5 [4];

    gen_poly( POLY_MASK(  5, 2, 0 ), sizeof poly5,  poly5  );

    uint32_t n = poly5 [3] * 0x1000000L + poly5 [2] * 0x10000L +

        poly5 [1] * 0x100L + poly5 [0];

    uint32_t rev = n & 1;

    for ( int i = 1; i < poly5_len; i++ )

      rev |= (n >> i & 1) << (poly5_len - i);

    debug_printf( "poly5: 0x%08lX\n", (long unsigned int)rev );

  }

}

Sap_Apu::Sap_Apu()

{

  impl = 0;

  for ( int i = 0; i < osc_count; i++ )

    osc_output( i, 0 );

}

void Sap_Apu::reset( Sap_Apu_Impl* new_impl )

{

  impl      = new_impl;

  last_time = 0;

  poly5_pos = 0;

  poly4_pos = 0;

  polym_pos = 0;

  control   = 0;

  for ( int i = 0; i < osc_count; i++ )

    memset( &oscs [i], 0, offsetof (osc_t,output) );

}

inline void Sap_Apu::calc_periods()

{

   // 15/64 kHz clock

  int divider = 28;

  if ( this->control & 1 )

    divider = 114;

  for ( int i = 0; i < osc_count; i++ )

  {

    osc_t* const osc = &oscs [i];

    int const osc_reload = osc->regs [0]; // cache

    int32_t period = (osc_reload + 1) * divider;

    static byte const fast_bits [osc_count] = { 1 << 6, 1 << 4, 1 << 5, 1 << 3 };

    if ( this->control & fast_bits [i] )

    {

      period = osc_reload + 4;

      if ( i & 1 )

      {

        period = osc_reload * 0x100L + osc [-1].regs [0] + 7;

        if ( !(this->control & fast_bits [i - 1]) )

          period = (period - 6) * divider;

        if ( (osc [-1].regs [1] & 0x1F) > 0x10 )

          debug_printf( "Use of slave channel in 16-bit mode not supported\n" );

      }

    }

    osc->period = period;

  }

}

void Sap_Apu::run_until( blip_time_t end_time )

{

  calc_periods();

  Sap_Apu_Impl* const impl = this->impl; // cache

  // 17/9-bit poly selection

  byte const* polym = impl->poly17;

  int polym_len = poly17_len;

  if ( this->control & 0x80 )

  {

    polym_len = poly9_len;

    polym = impl->poly9;

  }

  polym_pos %= polym_len;

  for ( int i = 0; i < osc_count; i++ )

  {

    osc_t* const osc = &oscs [i];

    blip_time_t time = last_time + osc->delay;

    blip_time_t const period = osc->period;

    // output

    Blip_Buffer* output = osc->output;

    if ( output )

    {

      output->set_modified();

      int const osc_control = osc->regs [1]; // cache

      int volume = (osc_control & 0x0F) * 2;

      if ( !volume || osc_control & 0x10 || // silent, DAC mode, or inaudible frequency

          ((osc_control & 0xA0) == 0xA0 && period < 1789773 / 2 / max_frequency) )

      {

        if ( !(osc_control & 0x10) )

          volume >>= 1; // inaudible frequency = half volume

        int delta = volume - osc->last_amp;

        if ( delta )

        {

          osc->last_amp = volume;

          impl->synth.offset( last_time, delta, output );

        }

        // TODO: doesn't maintain high pass flip-flop (very minor issue)

      }

      else

      {

        // high pass

        static byte const hipass_bits [osc_count] = { 1 << 2, 1 << 1, 0, 0 };

        blip_time_t period2 = 0; // unused if no high pass

        blip_time_t time2 = end_time;

        if ( this->control & hipass_bits [i] )

        {

          period2 = osc [2].period;

          time2 = last_time + osc [2].delay;

          if ( osc->invert )

          {

            // trick inner wave loop into inverting output

            osc->last_amp -= volume;

            volume = -volume;

          }

        }

        if ( time < end_time || time2 < end_time )

        {

          // poly source

          static byte const poly1 [] = { 0x55, 0x55 }; // square wave

          byte const* poly = poly1;

          int poly_len = 8 * sizeof poly1; // can be just 2 bits, but this is faster

          int poly_pos = osc->phase & 1;

          int poly_inc = 1;

          if ( !(osc_control & 0x20) )

          {

            poly     = polym;

            poly_len = polym_len;

            poly_pos = polym_pos;

            if ( osc_control & 0x40 )

            {

              poly     = impl->poly4;

              poly_len = poly4_len;

              poly_pos = poly4_pos;

            }

            poly_inc = period % poly_len;

            poly_pos = (poly_pos + osc->delay) % poly_len;

          }

          poly_inc -= poly_len; // allows more optimized inner loop below

          // square/poly5 wave

          uint32_t wave = poly5;

          check( poly5 & 1 ); // low bit is set for pure wave

          int poly5_inc = 0;

          if ( !(osc_control & 0x80) )

          {

            wave = run_poly5( wave, (osc->delay + poly5_pos) % poly5_len );

            poly5_inc = period % poly5_len;

          }

          // Run wave and high pass interleved with each catching up to the other.

          // Disabled high pass has no performance effect since inner wave loop

          // makes no compromise for high pass, and only runs once in that case.

          int osc_last_amp = osc->last_amp;

          do

          {

            // run high pass

            if ( time2 < time )

            {

              int delta = -osc_last_amp;

              if ( volume < 0 )

                delta += volume;

              if ( delta )

              {

                osc_last_amp += delta - volume;

                volume = -volume;

                impl->synth.offset( time2, delta, output );

              }

            }

            while ( time2 <= time ) // must advance *past* time to avoid hang

              time2 += period2;

            // run wave

            blip_time_t end = end_time;

            if ( end > time2 )

              end = time2;

            while ( time < end )

            {

              if ( wave & 1 )

              {

                int amp = volume & -(poly [poly_pos >> 3] >> (poly_pos & 7) & 1);

                if ( (poly_pos += poly_inc) < 0 )

                  poly_pos += poly_len;

                int delta = amp - osc_last_amp;

                if ( delta )

                {

                  osc_last_amp = amp;

                  impl->synth.offset( time, delta, output );

                }

              }

              wave = run_poly5( wave, poly5_inc );

              time += period;

            }

          }

          while ( time < end_time || time2 < end_time );

          osc->phase = poly_pos;

          osc->last_amp = osc_last_amp;

        }

        osc->invert = 0;

        if ( volume < 0 )

        {

          // undo inversion trickery

          osc->last_amp -= volume;

          osc->invert = 1;

        }

      }

    }

    // maintain divider

    blip_time_t remain = end_time - time;

    if ( remain > 0 )

    {

      int32_t count = (remain + period - 1) / period;

      osc->phase ^= count;

      time += count * period;

    }

    osc->delay = time - end_time;

  }

  // advance polies

  blip_time_t duration = end_time - last_time;

  last_time = end_time;

  poly4_pos = (poly4_pos + duration) % poly4_len;

  poly5_pos = (poly5_pos + duration) % poly5_len;

  polym_pos += duration; // will get %'d on next call

}

void Sap_Apu::write_data( blip_time_t time, unsigned addr, int data )

{

  run_until( time );

  int i = (addr ^ 0xD200) >> 1;

  if ( i < osc_count )

  {

    oscs [i].regs [addr & 1] = data;

  }

  else if ( addr == 0xD208 )

  {

    control = data;

  }

  else if ( addr == 0xD209 )

  {

    oscs [0].delay = 0;

    oscs [1].delay = 0;

    oscs [2].delay = 0;

    oscs [3].delay = 0;

  }

  /*

  // TODO: are polynomials reset in this case?

  else if ( addr == 0xD20F )

  {

    if ( (data & 3) == 0 )

      polym_pos = 0;

  }

  */

}

void Sap_Apu::end_frame( blip_time_t end_time )

{

  if ( end_time > last_time )

    run_until( end_time );

  last_time -= end_time;

}