#include "simpleprocessor32-bmp.h"

struct bitMapFileHeader{
  unsigned short bfType __attribute__((packed));
  unsigned int bfSize __attribute__((packed));
  unsigned short bfReserved1 __attribute__((packed));
  unsigned short bfReserved2 __attribute__((packed));
  unsigned int offBits __attribute__((packed));
};

struct bitMapInfoHeader{
  unsigned int biSize __attribute__((packed));
  unsigned int biWidth __attribute__((packed));
  unsigned int biHeight __attribute__((packed));
  unsigned short biPlanes __attribute__((packed));
  unsigned short biBitCount __attribute__((packed));
  unsigned int biCompression __attribute__((packed));
  unsigned int biSizeImage __attribute__((packed));
  unsigned int biXPelsPerMeter __attribute__((packed));
  unsigned int biYPelsPerMeter __attribute__((packed));
  unsigned int biClrUsed __attribute__((packed));
  unsigned int biClrImportant __attribute__((packed));
};

char chartbl[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};

int display_index;
int put_char(char d){
  *((char*)CHARACTER_DISPLAY+display_index) = d;
  display_index++;
}

void put_hex_digit(char a){
  put_char(chartbl[(a >> 4) & 0x0f]);
  put_char(chartbl[a & 0x0f]);
}

void put_short_hexstr(short a){
  put_char(chartbl[(a >> 12) & 0x0f]);
  put_char(chartbl[(a >> 8)  & 0x0f]);
  put_char(chartbl[(a >> 4) & 0x0f]);
  put_char(chartbl[a & 0x0f]);
}

void put_int_hexstr(int a){
  put_char(chartbl[(a >> 28) & 0x0f]);
  put_char(chartbl[(a >> 24) & 0x0f]);
  put_char(chartbl[(a >> 20) & 0x0f]);
  put_char(chartbl[(a >> 16) & 0x0f]);
  put_char(chartbl[(a >> 12) & 0x0f]);
  put_char(chartbl[(a >> 8)  & 0x0f]);
  put_char(chartbl[(a >> 4) & 0x0f]);
  put_char(chartbl[a & 0x0f]);
}

int hton_int(int a){
  int v = 0;
  v += ((a >> 24)&0x000000ff) <<  0;
  v += ((a >> 16)&0x000000ff) <<  8;
  v += ((a >>  8)&0x000000ff) << 16;
  v += ((a >>  0)&0x000000ff) << 24;
  return v;
}

short hton_short(short a){
  short v = 0;
  v += ((a >> 8)&0x00ff) << 0;
  v += ((a >> 0)&0x00ff) << 8;
  return v;
}


int get_line(struct bitMapInfoHeader *info)
{
  int l;
  l = hton_int(info->biWidth) * hton_short(info->biBitCount) / 8;
  if ((l % 4) != 0) {
	return ((l / 4) + 1) * 4;
  } else {
	return l;
  }
}

int get_offset(struct bitMapFileHeader *header,
			   struct bitMapInfoHeader *info,
			   int i)
{
  return hton_int(header->offBits) + get_line(info) * (hton_int(info->biHeight) - (i + 1));
}

void write_display(unsigned char * data, unsigned char * dest)
{
  int i, j;
  int pos;
  int height;
  int width;
  int line;
  unsigned char* da;
  struct bitMapFileHeader *header = (struct bitMapFileHeader *)data;
  struct bitMapInfoHeader *info = (struct bitMapInfoHeader *)(data+14);
  height = hton_int(info->biHeight)/2;
  width = hton_int(info->biWidth);
  for(i = 0; i < height; i++){
        da = dest + 3 * i * DISPLAY_WIDTH;
	pos = get_offset(header, info, i);
	for(j = 0; j < width; j++){
	  *(da+0) = *(data + pos + 2);
	  *(da+1) = *(data + pos + 1);
	  *(da+2) = *(data + pos + 0);
	  pos = pos + 3;
	  da = da + 3;
	}
  }
}


int main()
{
  unsigned char * src;
  unsigned char * dest;
  src = (unsigned char*)IMAGE_DATA;
  dest = (unsigned char*)DISPLAY_ADDR;
  write_display(src, dest);
}

