#include <sys/types.h>
#include <sys/mman.h>
#include <machine/console.h>
#include <ctype.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

static int xres = 1024, yres = 768;

void
adjbptr(int bptr)
{
	ioctl(0, CONS_SETWINORG, bptr);
}

#define	DP_START	1
#define DP_NEXT		2
#define DP_CLEAR	3
#define DP_SKIPTO	4

void
drawpixel16(void *buf, int flag, int r, int g, int b)
{
	static baseptr = -1;
	static off = 0;
	u_int16_t *cbuf = buf;

	switch (flag) {
	case DP_START:
		baseptr = off = 0;
		adjbptr(baseptr);
		break;
	case DP_NEXT:
		break;
	case DP_CLEAR:
		baseptr = off = 0;
		adjbptr(baseptr);
		return;
		break;
	case DP_SKIPTO:
		baseptr = (g * xres + r) * 2;
		off = (baseptr & 0xffff) / 2;
		baseptr &= ~0xffff;
		adjbptr(baseptr);
		return;
		break;
	}

	cbuf[off++] = (b >> 3) | ((g << 3) & 0x7e0) | ((r << 8) & 0xf800);

	if (off >= 65536 / 2) {
		baseptr += 65536;
		adjbptr(baseptr);
		off = 0;
	}
}

void
drawpixel32(void *buf, int x, int y, int r, int g, int b)
{
	static baseptr = -1;
	int nbaseptr;
	unsigned char *cbuf = buf;

	nbaseptr = y * xres * 4 + x * 4;
	nbaseptr &= ~65535;

	if (baseptr != nbaseptr)
		ioctl(0, CONS_SETWINORG, nbaseptr);

	baseptr = nbaseptr;

	cbuf[(y * xres + x) * 4 + 2 - baseptr] = r;
	cbuf[(y * xres + x) * 4 + 1 - baseptr] = g;
	cbuf[(y * xres + x) * 4 + 0 - baseptr] = b;
}

void
displayppm(int argc, char *argv[], void *buf)
{
	unsigned char fbuf[65536];
	int pos;
	int r;
	int m;
	int fd;
	int origrow, origcol;
	int row, col;
	int mval;
	int xpos, ypos;
	int oxpos, oypos;

	for (; argc; argc--, argv++) {
		if ((fd = open(*argv, O_RDONLY, 0)) == -1)
			continue;

		if ((r = read(fd, fbuf, sizeof fbuf)) == -1) {
			close(fd);
			continue;
		}

		if (sscanf(fbuf, "P6 %d %d %d", &origcol, &origrow, &mval)
		    != 3) {
			close(fd);
			continue;
		}

		/* skip over P6 */
		pos = 2;

		for (m = 3; m > 0; m--) {
			while (isspace(fbuf[pos])) pos++;
			while (isdigit(fbuf[pos])) pos++;
		}

		/* skip over LAST whitespace */
		pos++;

		for (oypos = ypos = 1; ypos <= row && ypos <= yres; ypos++) {
			drawpixel16(buf, DP_SKIPTO, 0, ypos - 1, 0);
			for (xpos = 1; xpos <= col && xpos <= xres; xpos++) {
				if (pos + 2 >= r) {
					/* reorginize buffer and read more */
					memmove(fbuf, fbuf + pos, r - pos);
					r = r - pos;
					pos = 0;
					r += m = read(fd, fbuf + r, sizeof fbuf
					    - r);
					if (m == 0 && r - pos < 3) {
						close(fd);
						continue;
					}
				}
				drawpixel16(buf, DP_NEXT, fbuf[pos + 0],
				    fbuf[pos + 1], fbuf[pos + 2]);
				pos += 3;
			}
		}
	}
}

void
frametest(void *buf)
{
	int x, y, f;

	for (f = 0; f < 30; f++) {
		drawpixel16(buf, DP_CLEAR, 0, 0, 0);
		for (y = 0; y < yres; y++)
			for (x = 0; x < xres; x++)
				drawpixel16(buf, DP_NEXT, x, y, f * 4);
	}
	exit(0);
}

void *
runmode(struct video_info info)
{
	char c;
	int mode;
	int fd;
	void *buf;
	int i;
	unsigned long l;

	if ((fd = open("/dev/mem", O_RDWR, 0)) == -1) {
		perror("open");
		return NULL;
	}

	if ((buf = mmap(0, info.vi_window_size, PROT_READ|PROT_WRITE, 0, fd,
	    info.vi_window)) == MAP_FAILED) {
		perror("mmap");
		return NULL;
	}

	close(fd);

	if (ioctl(0, KDSETMODE, KD_GRAPHICS) == -1) {
		perror("KDSETMODE");
		return NULL;
	}

	if (info.vi_mode >= M_VESA_BASE) {
		c = 'V';
		mode = info.vi_mode - M_VESA_BASE;
	} else {
		c = 'S';
		mode = info.vi_mode;
	}

	l = SW_VESA_64K_640;
	printf("%lx\n", l);
	l = _IO(c, mode);
	/*if (ioctl(0, SW_VESA_64K_640, 0) == -1) { */
	if (ioctl(0, _IO(c, mode), 0) == -1) {
		perror("switch mode");
		return NULL;
	}

	/*frametest(buf);*/

	return buf;
}

int
main(int argc, char *argv[])
{
	char c;
	char buf[512];
	void *sbuf;
	struct video_info info;

	memset(&info, 0, sizeof info);

	c = 0;
	info.vi_mode = 273;
	info.vi_width = xres;
	info.vi_height = yres;
	info.vi_depth = 16;
	info.vi_planes = 0;
	if (ioctl(0, c ? CONS_MODEINFO : CONS_FINDMODE , &info) == -1) {
		perror(c ? "CONS_MODEINFO" : "CONS_FINDMODE");
		exit(1);
	}

	printf("    mode#     flags   type    size       "
	       "font      window      linear buffer\n");
	printf("---------------------------------------"
	       "---------------------------------------\n");
	printf("%3d (0x%03x)", info.vi_mode, info.vi_mode);
	printf(" 0x%08x", info.vi_flags);
	if (info.vi_flags & V_INFO_GRAPHICS) {
		c = 'G';
		snprintf(buf, sizeof(buf), "%dx%dx%d %d",
			 info.vi_width, info.vi_height, 
			 info.vi_depth, info.vi_planes);
	} else {
		c = 'T';
		snprintf(buf, sizeof(buf), "%dx%d",
			 info.vi_width, info.vi_height);
	}
	printf(" %c %-15s", c, buf);
	snprintf(buf, sizeof(buf), "%dx%d", 
		 info.vi_cwidth, info.vi_cheight); 
	printf(" %-5s", buf);
	printf(" 0x%05x %2dk %2dk", 
	       info.vi_window, (int)info.vi_window_size/1024, 
	       (int)info.vi_window_gran/1024);
	printf(" 0x%08x %dk\n",
	       info.vi_buffer, (int)info.vi_buffer_size/1024);

	if ((sbuf = runmode(info)) == NULL)
		exit(1);

	displayppm(--argc, ++argv, sbuf);

	return 0;
}