/*
 * Unity utilities library
 *
 * Copyright (c) 2010 Benjamin Moody <floppusmaximus@users.sf.net>
 *
 * This program 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.
 *
 * 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
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include "pack.h"
#include "utils.h"

/*
static void pbin(FILE* f, unsigned int v, int nd)
{
  while (nd > 0) {
    nd--;
    fputc('0' + ((v >> nd) & 1), f);
  }
}
*/

static int get_packed_byte(unsigned int v)
{
  if (v < 27)
    return 0x59 + v;
  else if (v == 27)
    return 0x2C;		/* " */
  else if (v == 28)
    return 0x99;		/* pi */
  else if (v == 29)
    return 0x56;		/* space */
  else if (v == 30)
    return 0x1E;		/* , */
  else if (v == 31)
    return 0x45;		/* ? */
  else {
    fprintf(stderr, "invalid packed value %x\n", v);
    abort();
  }
}

static void pack_5_bits(unsigned int v, unsigned char** outbuf,
			unsigned int* size, unsigned int* size_a,
			unsigned int* sum)
{
  /*
  fprintf(stderr,"\n\t(c=%x) ", *sum);
  pbin(stderr, v, 5);
  */

  v = (v ^ *sum) & 0x1f;
  *sum ^= v;
  *sum = ((*sum << 3) | (*sum >> 5)) & 0xff;

  /*
  fprintf(stderr, "->");
  pbin(stderr, v, 5);
  fprintf(stderr,"\t(c=%x) ", *sum);
  */

  if (*size >= *size_a) {
    *size_a = *size + 1024;
    *outbuf = xrealloc(*outbuf, *size_a);
  }

  (*outbuf)[*size] = get_packed_byte(v);
  (*size)++;
}

void pack_asm_data(unsigned int load_addr, const unsigned char* data,
		   unsigned int data_size, unsigned char** packed,
		   unsigned int* packed_size)
{
  unsigned int size_a = 0;
  unsigned int sum, addr;
  unsigned int bitbuf, bitct;
  unsigned int i;

  *packed = NULL;
  *packed_size = 0;

  /* fprintf(stderr, " A:"); */
  sum = 0;
  pack_5_bits((load_addr >> 9), packed, packed_size, &size_a, &sum);
  pack_5_bits((load_addr >> 4), packed, packed_size, &size_a, &sum);
  pack_5_bits((load_addr << 1), packed, packed_size, &size_a, &sum);
  /* fprintf(stderr, "\n"); */

  //  sum = 0x80;
  addr = load_addr;
  while (data_size >= 9) {
    /* fprintf(stderr, " I:");
    for (i = 0; i < 9; i++) {
      fputc(' ', stderr);
      pbin(stderr, data[i], 8);
    }
    fprintf(stderr, "\n O:"); */

    //    sum = (sum & 0xe0) + 0x2A;
    bitbuf = bitct = 0;
    for (i = 0; i < 9; i++) {
      bitbuf = (bitbuf << 8) | data[i];
      bitct += 8;
      while (bitct >= 5) {
	bitct -= 5;
	pack_5_bits(bitbuf >> bitct, packed, packed_size, &size_a, &sum);
      }
    }

    //    bitbuf = (bitbuf << 8) | (sum & 0xe0);
    addr += 9;
    bitbuf = (bitbuf << 8) | (addr & 0xff);
    bitct += 8;
    while (bitct >= 5) {
      bitct -= 5;
      pack_5_bits(bitbuf >> bitct, packed, packed_size, &size_a, &sum);
    }

    /* fprintf(stderr, "\n"); */

    data += 9;
    data_size -= 9;
  }

  /* fprintf(stderr, " I:");
  for (i = 0; i < data_size; i++) {
    fputc(' ', stderr);
    pbin(stderr, data[i], 8);
  }
  fprintf(stderr, "\n O:"); */

  //  sum = (sum & 0xe0) + 0x2A;
  bitbuf = bitct = 0;
  for (i = 0; i < data_size; i++) {
    bitbuf = (bitbuf << 8) | data[i];
    bitct += 8;
    while (bitct >= 5) {
      bitct -= 5;
      pack_5_bits(bitbuf >> bitct, packed, packed_size, &size_a, &sum);
    }
  }
  if (bitct > 0)
    pack_5_bits(bitbuf << (5 - bitct), packed, packed_size, &size_a, &sum);
  pack_5_bits(0, packed, packed_size, &size_a, &sum);

  /* fprintf(stderr, "\n"); */
}