/*!
 * \file unpack_2bit_samples.cc
 *
 * \brief Unpacks 2 bit samples that have been packed into bytes or shorts
 * \author Cillian O'Driscoll cillian.odriscoll (at) gmail.com
 * -------------------------------------------------------------------------
 *
 * Copyright (C) 2010-2015  (see AUTHORS file for a list of contributors)
 *
 * GNSS-SDR is a software defined Global Navigation
 *          Satellite Systems receiver
 *
 * This file is part of GNSS-SDR.
 *
 * GNSS-SDR 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 of the License, or
 * (at your option) any later version.
 *
 * GNSS-SDR 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 GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
 *
 * -------------------------------------------------------------------------
 */


#include "unpack_2bit_samples.h"
#include <gnuradio/io_signature.h>

struct byte_2bit_struct
{
  signed sample_0:2;  // <- 2 bits wide only
  signed sample_1:2;  // <- 2 bits wide only
  signed sample_2:2;  // <- 2 bits wide only
  signed sample_3:2;  // <- 2 bits wide only
};

union byte_and_samples
{
    int8_t byte;
    byte_2bit_struct samples;
};

bool systemIsBigEndian()
{
    union
    {
        uint32_t i;
        char c[4];
    } test_int = {0x01020304};

    return test_int.c[0] == 1; 
}

bool systemBytesAreBigEndian()
{
    byte_and_samples b;
    b.byte = static_cast<int8_t>(0x01);
    if(*(char *)&b.byte == 1) return false;
    else return true;
}

void swapEndianness( int8_t const *in, std::vector< int8_t > &out, size_t item_size, unsigned int ninput_items )
{
    unsigned int i;
    unsigned int j = 0;
    int k = 0;
    int l = 0;
    size_t skip = item_size - 1;

    for( i = 0; i < ninput_items; ++i )
    {
        k = j + skip;
        l = j;
        while( k >= l )
        {
            out[j++] = in[k--];
        }
    }
}

unpack_2bit_samples_sptr make_unpack_2bit_samples( bool big_endian_bytes,
                                                   size_t item_size,
                                                   bool big_endian_items,
                                                   bool reverse_interleaving )
{
    return unpack_2bit_samples_sptr(
            new unpack_2bit_samples( big_endian_bytes,
                                     item_size,
                                     big_endian_items,
                                     reverse_interleaving )
            );
}

unpack_2bit_samples::unpack_2bit_samples( bool big_endian_bytes,
                                          size_t item_size,
                                          bool big_endian_items,
                                          bool reverse_interleaving )
    : sync_interpolator("unpack_2bit_samples",
                        gr::io_signature::make(1, 1, item_size),
                        gr::io_signature::make(1, 1, sizeof(char)),
                        4*item_size ), // we make 4 bytes out for every byte in
      big_endian_bytes_(big_endian_bytes),
      item_size_(item_size),
      big_endian_items_(big_endian_items),
      swap_endian_items_(false),
      reverse_interleaving_(reverse_interleaving)
{

    bool big_endian_system = systemIsBigEndian();

    // Only swap the item bytes if the item size > 1 byte and the system 
    // endianess is not the same as the item endianness:
    swap_endian_items_ = ( item_size_ > 1 ) && 
                         ( big_endian_system != big_endian_items);

    bool big_endian_bytes_system = systemBytesAreBigEndian();

    swap_endian_bytes_ = ( big_endian_bytes_system != big_endian_bytes_ );

}

unpack_2bit_samples::~unpack_2bit_samples()
{}

int unpack_2bit_samples::work(int noutput_items,
                                   gr_vector_const_void_star &input_items,
                                   gr_vector_void_star &output_items)
{
    signed char const *in = (signed char const *)input_items[0];
    int8_t *out = (int8_t*)output_items[0];

    size_t ninput_bytes = noutput_items/4;
    size_t ninput_items = ninput_bytes/item_size_;
    
    // Handle endian swap if needed
    if( swap_endian_items_ )
    {
        work_buffer_.reserve( ninput_bytes );
        swapEndianness( in, work_buffer_, item_size_, ninput_items );

        in = const_cast< signed char const *> ( &work_buffer_[0] );
    }

    // Here the in pointer can be interpreted as a stream of bytes to be
    // converted. But we now have two possibilities:
    // 1) The samples in a byte are in big endian order
    // 2) The samples in a byte are in little endian order

    byte_and_samples raw_byte;
    int n = 0;

    if( !reverse_interleaving_ )
    {
        if( swap_endian_bytes_ )
        {
            for(unsigned int i = 0; i < ninput_bytes; ++i)
            {
                // Read packed input sample (1 byte = 4 samples)
                raw_byte.byte = in[i];

                out[n++] = (int8_t)( 2*raw_byte.samples.sample_3 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_2 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_1 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_0 + 1 );

            }
        }
        else
        {
            for(unsigned int i = 0; i < ninput_bytes; ++i )
            {

                // Read packed input sample (1 byte = 4 samples)
                raw_byte.byte = in[i];

                out[n++] = (int8_t)( 2*raw_byte.samples.sample_0 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_1 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_2 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_3 + 1 );
            }
        }
    }
    else
    {

        if( swap_endian_bytes_ )
        {
            for(unsigned int i = 0; i < ninput_bytes; ++i)
            {
                // Read packed input sample (1 byte = 4 samples)
                raw_byte.byte = in[i];

                out[n++] = (int8_t)( 2*raw_byte.samples.sample_2 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_3 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_0 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_1 + 1 );

            }
        }
        else
        {
            for(unsigned int i = 0; i < ninput_bytes; ++i )
            {

                // Read packed input sample (1 byte = 4 samples)
                raw_byte.byte = in[i];

                out[n++] = (int8_t)( 2*raw_byte.samples.sample_1 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_0 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_3 + 1 );
                out[n++] = (int8_t)( 2*raw_byte.samples.sample_2 + 1 );
            }
        }
    }

    return noutput_items;
}

