/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   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 2, or (at your option)
   any later version.

### File Specific:
   main-programmer: Benjamin Wuest
   co-programmer: ...
*/


/* this is for debug output. It just says, that all calls to PRINT() belong to the DEBUG_MODULE_NETWORK module
   For more information refere to https://www.orxonox.net/cgi-bin/trac.cgi/wiki/DebugOutput
*/
#define DEBUG_MODULE_NETWORK

/* include your own header */
#include "converter.h"
#include "shell_command.h"

#include <limits>

SHELL_COMMAND_STATIC(debug, Converter, Converter::debug);


/* using namespace std is default, this needs to be here */
using namespace std;

/*!
 * Standard constructor
 */
Converter::Converter()
{
  /* set the class id for the base object */
  //this->setClassID(CL_ENTITY_MANAGER, "EntityManager");
}

/*!
 * Standard destructor
 */
Converter::~Converter()
{
}

const int sgnadd = 128; // = 2^7

/*!
 * Converts an int into a byte-array
 * @remarks: The int is stored in big-endian
 * @param x: The int which is to convert
 * @return: A byte-array that accords the given int value
 */
byte* Converter::intToByteArray(int x)
{
  const int mod = 256; // = 2^8


  byte* result = new byte[INTSIZE];
  int sgn;
  if (x >= 0)
    sgn = 1;
  else
  {
    sgn = -1;
    x = -x;
  }

  for (int i = 0; i < INTSIZE; i++)
  {
    result[i] = x % mod;
    x /= mod;
  }

  if (sgn == -1)
    result[INTSIZE - 1] += sgnadd;


  return result;
}

/*!
 * Converts an int into a byte-array and stores the result into a given byte-array
 * @remarks: The int is stored in big-endian
 * @param x: The int which is to convert
 * @return: The number of written bytes
 */
int Converter::intToByteArray(int x, byte* a, int length)
{
  if (length < INTSIZE)
  {
    PRINTF(1)("byte buffer to short to store an int. Needed length %i. Avaiable length %i", INTSIZE, length);
    return -1;
  }

  const int mod = 256; // = 2^8

  int sgn;
  if (x >= 0)
    sgn = 1;
  else
  {
    sgn = -1;
    x = -x;
  }

  for (int i = 0; i < INTSIZE; i++)
  {
    a[i] = x % mod;
    x /= mod;
  }

  if (sgn == -1)
    a[INTSIZE - 1] += sgnadd;

  return INTSIZE;
}

/*!
 * Converts a byte-array into an int
 * @param a: The byte-array which is to convert
 * @param x: The place where the result is stored
 * @return: The number of read bytes
 */
int Converter::byteArrayToInt(const byte* a, int* x)
{
  int mult = 1;
  const int step = 256; // = 2 ^ 8
  *x = 0;
  for (int i = 0; i < INTSIZE - 1; i++)
  {
    *x += a[i] * mult;
    mult *= step;
  }

  if (a[INTSIZE - 1] >= sgnadd)
  {
    *x += (a[INTSIZE - 1] - sgnadd) * mult;
    *x *= -1;
  }
  else
    *x += a[INTSIZE - 1] * mult;

  return INTSIZE;
}

/*!
 * Converts a float into a string containing its binary representation
 * @param x: The float which is to convert
 * @return: A string containing the float's binary representation
 */
char* Converter::floatToBinString(float x)
{
  char* result = new char[200];
  int pos = 0;

  if (x < 0)
  {
    result[pos++] = '-';
    x = -x;
  }

  float sub = 1;
  while (sub < x)
    sub *= 2;

  while ((x > 0 || sub >= 1) && pos < 200)
  {
    if (x >= sub)
    {
      result[pos] = '1';
      x -= sub;
    }
    else
      result[pos] = '0';
    pos++;
    sub /= 2;

    if (sub == 0.5f)
      result[pos++] = '.';
  }

  return result;
}

const int expmult = 8388608; //2^23

float Converter::getDenormConst()
{
  const int exp = 126;
  float result = 1.0f;
  for (int i = 0; i < exp; i++)
    result /= 2.0f;
  return result;
}

/*!
 * Converts a float value into a byte-array
 * @param x: The float which is to convert
 * @return: A byte-array which accords the given float
 */
byte* Converter::floatToByteArray(float x)
{
  byte* result = new byte[4];
  floatToByteArray(x, result, 4);
  return result;
  /*
  int mantisse = 0;
  int exponent = 128;

  int sgn;
  if (x < 0)
  {
    x = -x;
    sgn = -1;
  }
  else
    sgn = 1;

  if (x == 0)
  {
    exponent = 255;
    mantisse = 0;
  }
  else
  {
    //if (x < getDenormConst())
    //  printf("Denormalisiert!\n");
    //printf("DenormConst = %e", getDenormConst());

    float sub = 1;
    while (sub < x)
    {
      sub *= 2;
      exponent++;
    }

    while (x > 0)
    {
      if (x >= sub)
      {
        mantisse += 1;
        x -= sub;
      }

      mantisse *= 2;
      exponent--;
      sub /= 2;
    }
    exponent++;
    mantisse /= 2;

    printf("Conv:        mantisse = %i exponent = %i \n", mantisse, exponent);


    if (mantisse != 0)
    {
      while (mantisse < expmult)
      {
        mantisse *= 2;
        exponent--;
      }

      mantisse -= expmult;
    }
  }

  printf("Conv: mantisse = %i exponent = %i \n", mantisse, exponent);


  int hx = mantisse + expmult * exponent;
  byte* result = intToByteArray(hx);
  if (sgn == -1)
    result[3] += sgnadd;

  return result;
  */
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @return: A float value which accords the given byte-array
 */
float Converter::byteArrayToFloat(byte* a)
{
  byte* h = new byte[4];
  float result = 0.0f;
  byteArrayToFloat(a, &result);
  return result;
  /*
  int hexp = a[2] + a[3] * 256;
  int exponent = (hexp / 128) % 256;

  int hmant = a[0] + a[1] * 256 + a[2] * 65536;
  int mantisse = hmant % expmult;
  if (mantisse == 0 && exponent == 255)
    return 0;

  mantisse += expmult;
  exponent -= 128;


  int sgn;
  if (a[3] >= sgnadd)
    sgn = -1;
  else
    sgn = 1;

  printf("ReConv: mantisse = %i exponent = %i \n", mantisse, exponent);

  float emult = 1;
  if (exponent > 0)
    for (int i = 0; i < exponent; i++)
      emult *= 2;
  else if (exponent < 0)
    for (int i = 0; i > exponent; i--)
      emult /= 2;

  float result = mantisse * emult;
  if (sgn == -1)
    result = -result;

  return result;
  */
}

/*!
 * Converts a float value into a byte-array and stores the result into a given byte-array
 * @param x: The float which is to convert
 * @param a: The byte array where the result is to store
 * @param length: The length of the array a
 * @return: The number of written bytes
 */
int Converter::floatToByteArray(float x, byte* a, int length)
{
  if (length < FLOATSIZE)
  {
    PRINTF(1)("byte buffer to short to store a float. Needed length %i. Avaiable length %i", FLOATSIZE, length);
    return -1;
  }

  //handle 0 else function will loop for ever
  /*if ( x == 0 )
  {
    for ( int i = 0; i<FLOATSIZE; i++)
      a[i] = 0;
    return FLOATSIZE;
}*/

  int mantisse = 0;
  int exponent = 128;

  int sgn;
  if (x < 0)
  {
    x = -x;
    sgn = -1;
  }
  else
    sgn = 1;

  if (x == 0)
  {
    exponent = 255;
    mantisse = 0;
  }
  else
  {
    //if (x < getDenormConst())
    //  printf("Denormalisiert!\n");
    //printf("DenormConst = %e", getDenormConst());

    float sub = 1;
    while (sub < x)
    {
      sub *= 2;
      exponent++;
    }

    while (x > 0)
    {
      if (x >= sub)
      {
        mantisse += 1;
        x -= sub;
      }

      mantisse *= 2;
      exponent--;
      sub /= 2;
    }
    exponent++;
    mantisse /= 2;


///    printf("Conv:        mantisse = %i exponent = %i \n", mantisse, exponent);


    //if (mantisse != 0)
    //{
      while (mantisse < expmult)
      {
        mantisse *= 2;
        exponent--;
      }

      if (exponent >= 0)
        mantisse -= expmult;
      else
      {
        //Denormalized
        while (exponent < 0)
        {
          mantisse /= 2;
          exponent++;
        }
        printf("Conv: Denorm");
      }
    //}
  }

///  printf("Conv: mantisse = %i exponent = %i \n", mantisse, exponent);


  int hx = mantisse + expmult * exponent;
  //int result = intToByteArray(hx, a, length);
  intToByteArray(hx, a, length);
  if (sgn == -1)
    a[3] += sgnadd;


//  int hx = mantisse + expmult * exponent;
//  byte* result = intToByteArray(hx);
//  if (sgn == -1)
//    result[3] += sgnadd;

  //return result;
  return FLOATSIZE;
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @param x: The place where the result is to store
 * @return: The number of read bytes
 */
int Converter::byteArrayToFloat(const byte* a, float* x)
{
    //handle 0
  /*for (int i = 0; i<FLOATSIZE; i++)
  {
    if (a[i]!=0)
      break;
    if ( i==FLOATSIZE-1 )
    {
      *x = 0.0f;
      return FLOATSIZE;
    }
}*/

  int hexp = a[2] + a[3] * 256;
  int exponent = (hexp / 128) % 256;

  int hmant = a[0] + a[1] * 256 + a[2] * 65536;
  int mantisse = hmant % expmult;

  //handle 0
  if (mantisse == 0 && exponent == 255)
  {
    *x = 0;
    return FLOATSIZE;
  }
  else if (exponent == 0 && mantisse != 0)
  {
    exponent = -126;
    printf("ReConv: Denorm");
  }
  else
  {
    mantisse += expmult;
    exponent -= 128;
  }

  int sgn;
  if (a[3] >= sgnadd)
    sgn = -1;
  else
    sgn = 1;

///  printf("ReConv: mantisse = %i exponent = %i \n", mantisse, exponent);

  float emult = 1;
  if (exponent > 0)
    for (int i = 0; i < exponent; i++)
      emult *= 2;
  else if (exponent < 0)
    for (int i = 0; i > exponent; i--)
      emult /= 2;

  /*
  float result = mantisse * emult;
  if (sgn == -1)
    result = -result;

  return result;
  */

  *x = mantisse * emult;
  if (sgn == -1)
    *x = -  *x;

  return FLOATSIZE;
}

/*!
 * Converts a float value into a byte-array
 * @param x: The float which is to convert
 * @param a: The array where the result is to store
 * @param length: The length of the array a
 * @return: The number of written bytes
 */
int Converter::_floatToByteArray(float x, byte* a, int length)
{
  if ( length< FLOATSIZE )
  {
    PRINTF(1)("Byte Array to small\n");
    return 0;
  }
  byte* p = (byte*)&x;

  for (int i = 0; i < 4; i++)
    a[i] = p[i];
  
  return FLOATSIZE;
}


/*!
 * Converts a byte-array into a float value
 * @param a: The byte-array which is to convert
 * @param x: The place where the result is to store
 * @return: The number of read bytes
 */
int Converter::_byteArrayToFloat(const byte* a, float* x)
{
  *x = *((float*)a);

  return FLOATSIZE;
}










/**
 * copies a strint to a byte array
 * @param s: string to copy
 * @param a: byte array
 * @param length: string length
 * @return: the used number of bytes in byte array
 */
int Converter::stringToByteArray( const char * s, byte * a, int length, int maxLength )
{
  if ( length+INTSIZE > maxLength )
  {
    PRINTF(1)("Byte array is too small (%d) to store %d bytes\n", maxLength, length+INTSIZE);
    return -1;
  }

  int n = Converter::intToByteArray( length, a, maxLength );

  memcpy( a+INTSIZE, s, length );

  return length + INTSIZE;
}

/**
 * reads a string out of a byte array
 * @param a: byte array
 * @param s: string
 * @param maxLength: max bytes to copy
 * @return: the number of read bytes in byte array
 */
int Converter::byteArrayToString( const byte * a, char * s, int maxLength )
{
  int length;

  int n = Converter::byteArrayToInt( a, &length );


  if ( length+1 > maxLength )
  {
    PRINTF(1)("There is not enough space in string (%d) to store %d bytes\n", maxLength, length+1 );
    strncpy(s,"",maxLength);
    return -1;
  }

  memcpy( s, a+n, length );
  s[length] = '\0';

  return n+length;
}

/**
 * reads a string out of a byte array and allocates memory for string
 * @param a: byte array
 * @param s: string
 * @param maxLength: max bytes to copy
 * @return: the number of read bytes in byte array
 */
int Converter::byteArrayToStringM( const byte * a, char*& s )
{
  int length;

  int n = Converter::byteArrayToInt( a, &length );

  s = new char[length+1];

  if ( !s )
  {
    PRINTF(1)("Could not allocate memory!\n" );
    return -1;
  }

  memcpy( s, a+n, length );
  s[length] = '\0';

  return n+length;
}




void Converter::floatTest(float x)
{
  //float x = 8.0f;
  //float x = numeric_limits<float>::infinity();

  printf("To Convert: %e\n", x);

  byte* res = floatToByteArray(x);
  for (int i = 0; i < 4; i++)
    printf("%i ", res[i]);
  printf("\n");

  float y = byteArrayToFloat(res);
  printf("ReConvert: %e\n", y);

  if (x == y)
    printf("equal\n");
  else
    printf("different\n");
}

void Converter::ArrayfloatTest(float x)
{
  //float x = 8.0f;
  //float x = numeric_limits<float>::infinity();

  printf("To Convert: %e\n", x);
  byte* res = new byte[4];

  int wr = floatToByteArray(x, res, 4);
  for (int i = 0; i < 4; i++)
    printf("%i ", res[i]);
  printf("  written bytes: %i \n", wr);

  float y;
  int rd = byteArrayToFloat(res, &y);
  printf("ReConvert: %e\n", y);
  printf("Read bytes: %i   ->  ", rd);

  if (x == y)
    printf("equal\n");
  else
    printf("different\n");
}

void Converter::debug()
{
  printf("We rulez\n");

  ArrayfloatTest(0.125f);
  ArrayfloatTest(64.0f);
  ArrayfloatTest(0.0f);
  ArrayfloatTest(5.00091e-29f);

  //floatTest(-18.0098f);
  //floatTest(-24.07e23f);
  //floatTest(-0.0f);
  //floatTest(-5.67e-29f);
  
  
  //floatTest(-45.7e-32f);
  //floatTest(45.7e-33f);
  //floatTest(-45.7e-34f);
  //floatTest(45.7e-35f);
}