#include <devices/vid_mode.h>
#include <ubixos/io.h>

struct video_mode vidmode;

static unsigned char* vmem=(unsigned char*)0xA0000;
static unsigned char active_color;
static unsigned long active_offset;

vga_mode vga_640x480x4 =
{
  {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x14, 0x07, 0x38, 0x39, 0x3A, 0x3B,
   0x3C, 0x3D, 0x3E, 0x3F, 0x01, 0x00, 0x0F, 0x00, 0x00},
  {0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0x0B, 0x3E, 0x00, 0x40, 0x00, 0x00,
   0x00, 0x00, 0x00, 0x00, 0xEA, 0x8C, 0xDF, 0x28, 0x00, 0xE7, 0x04, 0xE3},
  {0x07, 0x00, 0x00, 0x00, 0x03, 0x03, 0x05, 0x0F, 0xFF},
   0xE3,
  {0x03, 0x01, 0x0F, 0x00, 0x06}
};


void init_mode_0x11(void) {
  vidmode.maxx=640;
  vidmode.maxy=480;
  vidmode.bpp=8;
  vidmode.SetColor=(void *)drvVGA13SetColor;
  vidmode.GetDrvColor=drvVGetDrvColor;
  vidmode.PutPixel=(void *)vga_putpixel;
  vidmode.GetPixel=(void *)vga_getpixel;
  vidmode.SolidLine=vga_putline;
  vidmode.FillRect=(void *)defVGAFillRect;
  vga_setregs(&vga_640x480x4);
  outportw(0x3ce,0x0305);
  inportb(0x3da);
  }

void drvVGA13SetColor(__u32 nr, COLORREF color) {
  outportb(PaletteMask,0xFF);
  outportb(PaletteRegisterWrite,nr);
  outportb(PaletteData, GetRValue(color)/4);
  outportb(PaletteData, GetGValue(color)/4);
  outportb(PaletteData, GetBValue(color)/4);
  }

COLORREF sysPal[16]= {
  RGB(0,0,0),                /* black  */
  RGB(0,0,128),            /* blue   */
  RGB(0,128,0),            /* green  */
  RGB(0,128,128),        /* cyjan  */
  RGB(128,0,0),            /* red    */
  RGB(128,0,128),        /* Magenta */
  RGB(128,128,0),        /* brown   */
  RGB(128,128,128),    /* gary    */
  RGB(192,192,192),    /* gray    */
  RGB(0,0,255),            /* blue    */
  RGB(0,255,0),            /* green   */
  RGB(0,255,255),        /* cyjan   */
  RGB(255,0,0),            /* red     */
  RGB(255,0,255),        /* Magenta  */
  RGB(255,255,0),        /* brown   */
  RGB(255,255,255)     /* white   */
};

COLORREF  drvVGetDrvColor (COLORREF color) {
 long i=0;
 long odl=0, min=0, nrmin=0;
 /* Not needed for 32bpp and 24bpp modes */
 if(vidmode.bpp==32 || vidmode.bpp==24) return color;
 odl=0xFFFFFFFE;
 for (i=0; i<16; i++)
 {
   if (sysPal[i]==color) return i;
   // liczenie odleglosci
    odl= (GetRValue(sysPal[i])-GetRValue(color)) * (GetRValue(sysPal[i])-GetRValue(color))+
    (GetGValue(sysPal[i])-GetGValue(color)) * (GetGValue(sysPal[i])-GetGValue(color))+
    (GetBValue(sysPal[i])-GetBValue(color)) * (GetBValue(sysPal[i])-GetBValue(color));
  if (min>odl) { min=odl; nrmin=i; }
 }
 return i;
}

static void vga_putpixel(__u32 x,__u32 y,COLORREF color) {
  unsigned char l,mask;
  if (x<0) {
    return;
    }
  else {
    if (x>vidmode.maxx-1) {
      return;
      }
    }
  if (y<0) {
    return;
    }
  else if (y>vidmode.maxy-1) {
    return;
    }
  mask=0x80>>(x&7);
  set_color(color);
  l=vmem[(y*vidmode.maxx+x)>>3];
  vmem[(y*vidmode.maxx+x)>>3]=mask;
  }

static void vga_putline(__u32 x1,__u32 y1,__u32 x2,__u32 y2,COLORREF color)
{
 int t,dx,dy;
 unsigned long voff;
 unsigned char l,mask;
 int yincr;
 int xincr;
 int d;
 int aincr;
 int bincr;
 int x;
 int y;
 if(x1==x2)
 {
  sort(y1,y2);
  if(x1>(vidmode.maxx-1) || x1<0)
   return;
  if(y1>(vidmode.maxy-1) && y2>(vidmode.maxx-1))
   return;
  if(y1<0 && y2<0)
   return;
  vga_clip(x1,y1);
  vga_clip(x2,y2);
  set_color(color);
  voff=(y1*vidmode.maxx+x1)>>3;
  mask=0x80>>(x1&7);
  for(t=y1;t<=y2;t++,voff+=(vidmode.maxx>>3))
  {
   l=vmem[voff];
   vmem[voff]=mask;
  }
  return;
 }
 if(y1==y2)
 {
  sort(x1,x2);
  if(y1>(vidmode.maxy-1) || y1<0)
   return;
  if(x1>(vidmode.maxx-1) && x2>(vidmode.maxx-1))
   return;
  if(x1<0 && x2<0)
   return;
  vga_clip(x1,y1);
  vga_clip(x2,y2);
  set_color(color);
  voff=(y1*vidmode.maxx+x1)>>3;
  mask=0xFF>>(x1&7);
  if((x2-x1)<8)
  {
   mask^=(0xFF>>((x2&7)+1));
   l=vmem[voff];
   vmem[voff]=mask;
   return;
  }
  l=vmem[voff];
  vmem[voff]=mask;
  voff++;
  t=x1+8-(x1&7);
  for(;t+8<=x2;t+=8)
  {
   l=vmem[voff];
   vmem[voff]=0xFF;
   voff++;
  }
  mask=0xFF<<(7-(x2&7));
  l=vmem[voff];
  vmem[voff]=mask;
  return;
 }
 dx=absolute(x2-x1);
 dy=absolute(y2-y1);
 if(dx>=dy)
 {
  if(x1>x2)
  {
   swap(x1,x2);
   swap(y1,y2);
  }
  yincr=1;
  if(y2<y1) yincr=-yincr;
  d=2*dy-dx;
  aincr=2*(dy-dx);
  bincr=2*dy;
  x=x1;
  y=y1;
  vga_putpixel(x,y,color);
  while(++x<=x2)
  {
   if(d>=0)
   {
    y+=yincr;
    d+=aincr;
   } else d+=bincr;
   vga_putpixel(x,y,color);
  }
 } else {
  if(y1>y2)
  {
   swap(x1,x2);
   swap(y1,y2);
  }
  xincr=1;
  if(x2<x1) xincr=-xincr;
  d=2*dx-dy;
  aincr=2*(dx-dy);
  bincr=2*dx;
  x=x1;
  y=y1;
  vga_putpixel(x,y,color);
  while(++y<=y2) {
   if(d>=0) {
    x+=xincr;
    d+=aincr;
   } else
   d+=bincr;   vga_putpixel(x,y,color);
  }
 }
}


static void vga_setregs(vga_mode* mode) {
  unsigned long n;
  inportb(0x3da);
  outportb(0x3c0,0x00);
  outportb(0x3c2,mode->misc|1);
  for(n=0;n<5;n++) {
    outportw(0x3c4,(mode->sequencer[n]<<8)|n);
    }
  outportw(0x3d4,((mode->crtc[17]&0x7f)<<8)|17);
  for(n=0;n<25;n++) {
    outportw(0x3d4,(mode->crtc[n]<<8)|n);
    }
  for(n=0;n<9;n++) {
    outportw(0x3ce,(mode->graphics[n]<<8)|n);
    }
  for(n=0;n<21;n++) {
    inportb(0x3da);
    outportb(0x3c0,n);
    outportb(0x3c0,mode->attribute[n]);
    }
  inportb(0x3da);
  outportb(0x3c0,0x20);
  outportw(0x3ce,0x0305);
  inportb(0x3da);
  }

static COLORREF vga_getpixel(__u32 x,__u32 y) {
  unsigned char col,mask;
  unsigned long voff;
  int n;
  unsigned char planes[4];
  if (x<0) {
    return(-1);
    }
  else {
    if (x>vidmode.maxx-1) {
      return(-1);
      }
    }
 if(y<0) return;
  else if(y>vidmode.maxy-1)
   return;
 col=0;
 voff=(y*vidmode.maxx+x)>>3;
 mask=0x80>>(x&7);
 if(active_offset!=voff)
 {
  for(n=0;n<4;n++)
  {
   outportw(0x3ce,(n<<8)|4);
   inportb(0x3da);
   planes[n]=vmem[voff];
  }
  active_offset=voff;
 }
 for(n=3;n>=0;n--)
 {
  col<<=1;
  if(planes[n]&mask)
   col++;
 }
 return (COLORREF)col;
}

void defVGAFillRect(__u32 xp, __u32 yp, __u32 xk, __u32 yk, COLORREF color)
{
 long x, y;
 for (y=yp; y<=yk; y++)
  for (x=xp; x<=xk; x++)
   vidmode.PutPixel(x, y, color);
}