#include <malloc.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <spu_mfcio.h>
#include "SpuDraw.h"
#include "polygon_pack.h"
#include "span_pack.h"
#include "spu_span.h"
#include "error.h"
#include "CellScheduler.h"

#define PIXELSIZE 11520 //32 line pixel size

SpuDraw::~SpuDraw(void)
{
     free(zRow);
     free(linebuf);
}

void
SpuDraw::linebuf_init()
{
     int y = 8;
     int x = IMG_MAX_X;

     linebuf = (int*)memalign(DEFAULT_ALIGNMENT, sizeof(int)*x*y);
     memset(linebuf, 0, sizeof(int)*x*y);
}

void
SpuDraw::zRow_init(void)
{
     int y = 8;
     int x = IMG_MAX_X;
     float z = 65535;

     zRow = (float*)malloc(sizeof(float)*x*y);
     memset(zRow, *(int*)&z, sizeof(float)*x*y);
}


char*
SpuDraw::get_pixel(int tx, int ty, void *texture_image)
{
     //return (char*)texture_image->pixels+(texture_image->format->BytesPerPixel*((texture_image->w)*ty+tx));
     return (char*)texture_image+(3*((128)*ty+tx));
}

Uint32
SpuDraw::SDL_MapRGB(SDL_PixelFormat *format,
		    Uint8 r, Uint8 g, Uint8 b)
{
     if ( format->palette == NULL ) {
	  return (r >> format->Rloss) << format->Rshift
	       | (g >> format->Gloss) << format->Gshift
	       | (b >> format->Bloss) << format->Bshift
	       | format->Amask;
     } else {
	  return SDL_FindColor(format->palette, r, g, b);
     }
}

Uint8
SpuDraw::SDL_FindColor(SDL_Palette *pal, Uint8 r, Uint8 g, Uint8 b)
{
     /* Do colorspace distance matching */
     unsigned int smallest;
     unsigned int distance;
     int rd, gd, bd;
     int i;
     Uint8 pixel=0;

     smallest = ~0;
     for ( i=0; i<pal->ncolors; ++i ) {
	  rd = pal->colors[i].r - r;
	  gd = pal->colors[i].g - g;
	  bd = pal->colors[i].b - b;
	  distance = (rd*rd)+(gd*gd)+(bd*bd);
	  if ( distance < smallest ) {
	       pixel = i;
	       if ( distance == 0 ) { /* Perfect match! */
		    break;
	       }
	       smallest = distance;
	  }
     }
     return(pixel);
}

Uint32
SpuDraw::get_rgb(int tx, int ty, void *texture)
{
#if 1
     Uint8 red, green, blue, alpha;

     if (tx<0) tx = 0;
     if (128-1< tx) tx = 128-1 ;
     if (ty<0) ty = 0;
     if (128-1< ty) ty = 128-1 ;

     char *p = get_pixel(tx,ty,texture);

     blue  = (Uint8) p[0];
     green = (Uint8) p[1];
     red   = (Uint8) p[2];
     
     SDL_PixelFormat pf;
     pf.BitsPerPixel = 32;
     pf.BytesPerPixel = 4;
     alpha = 255;

     return (red & 0xff) * 0x10000 + (green & 0xff) * 0x100 + (blue & 0xff)
	  + (alpha << 24);
     //return SDL_MapRGB(&pf, red, green, blue);
#else
     return 0x900000;
#endif
}

int
SpuDraw::run(void *rbuf, void *writebuf)
{
     SPANPACK_PTR sp = (SPANPACK_PTR)rbuf;
     unsigned int fbdev_addr = task->out_addr;
     SPAN_PTR span;
     int ofset_x = 320;
     int ofset_y = 240;
     int width = IMG_MAX_X;
     
     __debug("[SpuDraw]:%s\n", __FUNCTION__);

     int render_y = 0;

     zRow_init();
     linebuf_init();

     //void *texture = CellScheduler::tex;

     render_y = sp->span[0].y;
     render_y += 1080/2;
     render_y = (render_y/8)*8;
          
     for (int t = 0; t < sp->info.size; t++) {	  
	  span = &sp->span[t];

	  //__debug("span x = %d, y = %d\n", span->x, span->y);
	  int end = span->length_x;
	  Uint32 rgb = 0x00ff00;
	  float tex1 = span->tex_x1;
	  float tex2 = span->tex_x2;
	  float tey1 = span->tex_y1;
	  float tey2 = span->tex_y2;
	  int tex_xpos;
	  int tex_ypos;
	  int tex_zpos;
	  int x = span->x + ofset_x;
	  int y = span->y + ofset_y;
	  float z = span->start_z;
	  float zpos = span->end_z;
	  float tex_x, tex_y, tex_z;

	  if (end == 1) {
	       tex_xpos = (int)((span->tex_height-1) * tex1);
	       tex_ypos = (int)((span->tex_width-1) * tey1);
	       tex_zpos = (int)z;

	       if (z < zRow[x + (width*(y%8))]) {
		    rgb = get_rgb(tex_xpos, tex_ypos, CellScheduler::tex);
		    zRow[x + (width*(y%8))] = z;
		    linebuf[(width*(y%8)) + x] = rgb;
		    //viewer->write_pixel(x,y,zpos,rgb);
	       }
	  } else {
	       for (int j = 0; j < end; j++) {
		    tex_x = tex1*(end-1-j)/(end-1) + tex2*j/(end-1);
		    tex_y = tey1*(end-1-j)/(end-1) + tey2*j/(end-1);
		    tex_z = z*(end-1-j)/(end-1) + zpos*j/(end-1);
		    if (tex_x > 1) tex_x = 1;
		    if (tex_y > 1) tex_y = 1;
		    tex_xpos = (int)((span->tex_height-1) * tex_x);
		    tex_ypos = (int)((span->tex_width-1) * tex_y);

		    if (z < zRow[x + j + (width*(y%8))]) {
			 //__debug("x = %d, y = %d, width = %d, end = %d\n", x, y, width, end);
			 rgb = get_rgb(tex_xpos,tex_ypos,CellScheduler::tex);
			 zRow[j + x + (width*(y%8))] = z;
			 linebuf[j + (width*(y%8)) + x] = rgb;
			 //viewer->write_pixel(j+x,y,tex_z,rgb);
		    }
	       }
	  }
     }
     __debug("[SpuDraw]:run finish!!\n");
     writebuffer(0, fbdev_addr, render_y);

     free(rbuf);
     return 0;
}

void
SpuDraw::writebuffer(int i, unsigned int fbdev_addr, int y) {
     for (i = 0; i < 8; i++) {
	  spu_mfcdma32(&linebuf[i*IMG_MAX_X], fbdev_addr + (4*1920*(y+i)),
		       sizeof(int)*IMG_MAX_X, i+5, MFC_PUT_CMD);
	  connector->dma_wait(i+5);
     }
}


SchedTask*
createTask_spuDraw(TaskListPtr _taskList, TaskPtr _task,
		   void *rbuff, void *wbuff, DmaManager *dma)
{
     rbuff = memalign(16, sizeof(SPANPACK));

     return new SpuDraw(_taskList, _task, rbuff, wbuff, dma);
}