/* -*- c++ -*- */

/*

 * Copyright 2004 Free Software Foundation, Inc.

 * 

 * This file is part of GNU Radio

 * 

 * GNU Radio 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 3, or (at your option)

 * any later version.

 * 

 * GNU Radio 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 Radio; see the file COPYING.  If not, write to

 * the Free Software Foundation, Inc., 51 Franklin Street,

 * Boston, MA 02110-1301, USA.

 */

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#include <gr_simple_correlator.h>

#include <gr_simple_framer_sync.h>

#include <gr_io_signature.h>

#include <assert.h>

#include <stdexcept>

#include <gr_count_bits.h>

#include <string.h>

static const int THRESHOLD = 3;

gr_simple_correlator_sptr

gr_make_simple_correlator (int payload_bytesize)

{

  return gr_simple_correlator_sptr (new gr_simple_correlator (payload_bytesize));

}

gr_simple_correlator::gr_simple_correlator (int payload_bytesize)

  : gr_block ("simple_correlator",

              gr_make_io_signature (1, 1, sizeof (float)),

              gr_make_io_signature (1, 1, sizeof (unsigned char))),

    d_payload_bytesize (payload_bytesize),

    d_state (ST_LOOKING), d_osi (0),

    d_bblen ((payload_bytesize + GRSF_PAYLOAD_OVERHEAD) * GRSF_BITS_PER_BYTE),

    d_bitbuf (new unsigned char [d_bblen]),

    d_bbi (0)

{

  d_avbi = 0;

  d_accum = 0.0;

  d_avg = 0.0;

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

    d_avgbuf[i] = 0.0;

#ifdef DEBUG_SIMPLE_CORRELATOR

  d_debug_fp = fopen("corr.log", "w");

#endif

  enter_looking ();

}

gr_simple_correlator::~gr_simple_correlator ()

{

#ifdef DEBUG_SIMPLE_CORRELATOR

  fclose(d_debug_fp);

#endif  

  delete [] d_bitbuf;

}

void

gr_simple_correlator::enter_looking ()

{

  fflush (stdout);

  // fprintf (stderr, ">>> enter_looking\n");

  d_state = ST_LOOKING;

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

    d_shift_reg[i] = 0;

  d_osi = 0;

  d_avbi = 0;

  d_avg = d_avg * 0.5;

  d_accum = 0;

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

    d_avgbuf[i] = 0.0;

}

void

gr_simple_correlator::enter_under_threshold ()

{

  fflush (stdout);

  // fprintf (stderr, ">>> enter_under_threshold\n");

  d_state = ST_UNDER_THRESHOLD;

  d_transition_osi = d_osi;

}

void

gr_simple_correlator::enter_locked ()

{

  d_state = ST_LOCKED;

  int delta = sub_index (d_osi, d_transition_osi);

  d_center_osi = add_index (d_transition_osi, delta/2);

  d_center_osi = add_index (d_center_osi, 3);   // FIXME

  d_bbi = 0;

  fflush (stdout);

  // fprintf (stderr, ">>> enter_locked  d_center_osi = %d\n", d_center_osi);

  d_avg = std::max(-1.0, std::min(1.0, d_accum * (1.0/AVG_PERIOD)));

  // fprintf(stderr, ">>> enter_locked  d_avg = %g\n", d_avg);

}

static void

packit (unsigned char *pktbuf, const unsigned char *bitbuf, int bitcount)

{

  for (int i = 0; i < bitcount; i += 8){

    int t = bitbuf[i+0] & 0x1;

    t = (t << 1) | (bitbuf[i+1] & 0x1);

    t = (t << 1) | (bitbuf[i+2] & 0x1);

    t = (t << 1) | (bitbuf[i+3] & 0x1);

    t = (t << 1) | (bitbuf[i+4] & 0x1);

    t = (t << 1) | (bitbuf[i+5] & 0x1);

    t = (t << 1) | (bitbuf[i+6] & 0x1);

    t = (t << 1) | (bitbuf[i+7] & 0x1);

    *pktbuf++ = t;

  }

}

void

gr_simple_correlator::update_avg(float x)

{

  d_accum -= d_avgbuf[d_avbi];

  d_avgbuf[d_avbi] = x;

  d_accum += x;

  d_avbi = (d_avbi + 1) & (AVG_PERIOD-1);

}

int

gr_simple_correlator::general_work (int noutput_items,

                                    gr_vector_int &ninput_items,

                                    gr_vector_const_void_star &input_items,

                                    gr_vector_void_star &output_items)

{

  const float *in = (const float *) input_items[0];

  unsigned char *out = (unsigned char *) output_items[0];

  int n = 0;

  int nin = ninput_items[0];

  int decision;

  int hamming_dist;

  struct debug_data {

    float        raw_data;

    float        sampled;

    float         enter_locked;

  } debug_data;

  while (n < nin){

#ifdef DEBUG_SIMPLE_CORRELATOR

    debug_data.raw_data = in[n];

    debug_data.sampled = 0.0;

    debug_data.enter_locked = 0.0;

#endif

    switch (d_state){

    case ST_LOCKED:

      if (d_osi == d_center_osi){

#ifdef DEBUG_SIMPLE_CORRELATOR

        debug_data.sampled = 1.0;

#endif

        decision = slice (in[n]);

        d_bitbuf[d_bbi] = decision;

        d_bbi++;

        if (d_bbi >= d_bblen){

          // printf ("got whole packet\n");

          unsigned char pktbuf[d_bblen/GRSF_BITS_PER_BYTE];

          packit (pktbuf, d_bitbuf, d_bbi);

          printf ("seqno %3d\n", pktbuf[0]);

          memcpy (out, &pktbuf[GRSF_PAYLOAD_OVERHEAD], d_payload_bytesize);

          enter_looking ();

          consume_each (n + 1);

          return d_payload_bytesize;

        }

      }

      break;

    case ST_LOOKING:

    case ST_UNDER_THRESHOLD:

      update_avg(in[n]);

      decision = slice (in[n]);

      d_shift_reg[d_osi] = (d_shift_reg[d_osi] << 1) | decision;

      hamming_dist = gr_count_bits64 (d_shift_reg[d_osi] ^ GRSF_SYNC);

      // printf ("%2d  %d\n", hamming_dist, d_osi);

      if (d_state == ST_LOOKING && hamming_dist <= THRESHOLD){

        // We're seeing a good PN code, remember location

        enter_under_threshold ();

      }

      else if (d_state == ST_UNDER_THRESHOLD && hamming_dist > THRESHOLD){

        // no longer seeing good PN code, compute center of goodness

        enter_locked ();

        debug_data.enter_locked = 1.0;

      }

      break;

    default:

      assert (0);

    }

#ifdef DEBUG_SIMPLE_CORRELATOR

    fwrite(&debug_data, sizeof (debug_data), 1, d_debug_fp);

#endif

    d_osi = add_index (d_osi, 1);

    n++;

  }

  consume_each (n);

  return 0;

}