#include <stdlib.h>
#include <stdio.h>
#include <vga.h>
#include <time.h>
#include <math.h>

#define VIDEO_MODE G640x480x64K

#define VIDEO_SCREEN_WIDTH 640UL
#define VIDEO_SCREEN_HEIGHT 480UL
#define VIDEO_PAGESIZE (VIDEO_SCREEN_WIDTH*VIDEO_SCREEN_HEIGHT*2)

#define RGB16(r,g,b) ( (((r)>>3)<< 11) | (((g)>>2)<<5) | ((b)>>3) )

#define VIDEO_PIXEL(sbuf,x,y) \
  ((unsigned short *) &(sbuf[2*((y)*VIDEO_SCREEN_WIDTH+(x))]))

void grid32(unsigned char *buf) {
  unsigned short *pixel;
  static double a = 0.0;
  int x0;
  int y0;
  int x;
  int y;
  int ix;
  int iy;

  x0 = (int) (640*sin(a));
  y0 = (int) (480*cos(a));
  a += 0.01;

  for (iy=0; iy<VIDEO_SCREEN_HEIGHT;iy+=1) {
    for (ix=0; ix<VIDEO_SCREEN_WIDTH;ix+=1) {
      pixel = VIDEO_PIXEL(buf,ix,iy);
      x = ix+x0;
      y = iy+y0;

      if (!(x%32) || !(y%32)) {
        *pixel = RGB16(127,0,0);
      } else if (!((y-(x>>1))%64)) {
        *pixel = RGB16(0,127,0);
      } else if (!((y+(x>>1))%64)) {
        *pixel = RGB16(0,127,0);
      }
    }
  }
}

  
int get_elapsed_ms(void) {
  static struct timeval start = {0, 0};
  struct timeval now;
  int s,us;

  if (start.tv_sec ==0 && start.tv_usec==0) {
    gettimeofday(&start, NULL);
    return 0;
  }
  gettimeofday(&now, NULL);
  s = now.tv_sec - start.tv_sec;
  us = now.tv_usec - start.tv_usec;

  return s*1000 + us/1000;
}

inline void MemClear(void *p, unsigned long len, unsigned long c=0) {
  c = c<<24 | c<<16 | c<<8 | c;
  while(len>=4) {
    *((unsigned long *)p) = c;
    len-=4;
    p = (void *) ( ((unsigned long)p) + 4 );
  }
  while (len>0) {
    *((char *)p) = c;
    len-=1;
    p = (void *) ( ((unsigned long)p) + 1 );
  }
}

inline void MemCopy(void *d, void *s, unsigned long len) {
  while(len>=4) {
    *((unsigned long *)d) = *((unsigned long *)s);
    len-=4;
    s = (void *) ( ((unsigned long)s) + 4 );
    d = (void *) ( ((unsigned long)d) + 4 );
  }
  while (len>0) {
    *((char *)d) = *((char *)s);
    len-=1;
    s = (void *) ( ((unsigned long)s) + 1 );
    d = (void *) ( ((unsigned long)d) + 1 );
  }
}

void shutdown() {
  exit(0);
}

int main(void) {

  int frame = 0;
  double fps;
  unsigned char *vbuf;
  int pageoffset[] = { 0, VIDEO_PAGESIZE };
  int writepage = 1;
  int c;
  struct timeval tv_start;
  struct timeval tv;

  vga_safety_fork(shutdown);
  vga_init();
  vga_lockvc();
  vga_setmode(VIDEO_MODE);
  vga_clear();
  vga_setlinearaddressing();
  vga_claimvideomemory(2 * VIDEO_PAGESIZE);
  vbuf = vga_getgraphmem();

  writepage = 1;
  vga_setdisplaystart(pageoffset[0]);

  MemClear(vbuf, 2 * VIDEO_PAGESIZE);

  get_elapsed_ms();
  for(;;) {
    
    MemClear(vbuf + pageoffset[writepage], VIDEO_PAGESIZE, 0x00);

    grid32(vbuf + pageoffset[writepage]);

    frame+=1;
    vga_waitretrace();
    vga_setdisplaystart(pageoffset[writepage]);
    writepage ^= 1;

    if (vga_getkey()) break;
  }

  fps = (double)frame * 1000.0 / get_elapsed_ms();
   
  printf("%lf fps\n", fps);
 
  vga_clear();
  vga_setmode(TEXT);
  vga_unlockvc();

  return EXIT_SUCCESS;
}