#include <pspkernel.h>
#include <pspdisplay.h>
#include <stdio.h>
#include <pspctrl.h>
#include "OURIMAGE.H"

PSP_MODULE_INFO("imageblit", 0x1000, 1, 1);
PSP_MAIN_THREAD_ATTR(0);


//Globals
int pmode, pwidth, pheight, bufferwidth, pixelformat;
unsigned int* vram32;
unsigned int* vram16;
   SceCtrlData pad;
   extern unsigned char ourImage[];

//Somewhere in your code

   sceDisplayGetMode(&pmode, &pwidth, &pheight);
   sceDisplayGetFrameBuf((void*)&vram32, &bufferwidth, &pixelformat, PSP_DISPLAY_SETBUF_IMMEDIATE);
   vram16 = (unsigned int*) vram32;
   
void drawimage(unsigned char * image, unsigned int srcw, unsigned int srch, unsigned int destx, unsigned int desty)
{
   #define sbpp 4 //Source bytes-per-pixel (RGBA=4)
   unsigned int x, y, pos = 0;
   
   sceDisplayWaitVblankStart();

   for(y = desty; y < desty+srch; y++){
      for(x = destx; x < destx+srcw; x++){
         unsigned int color, offset = x + y*bufferwidth;
         unsigned char r = 0, g = 0, b = 0;
         
         switch (pixelformat) {
            case 0:   // 16-bit RGB 5:6:5
               color = vram16[offset];
               r = (color & 0x1f) << 3;
               g = ((color >> 5) & 0x3f) << 2;
               b = ((color >> 11) & 0x1f) << 3;
               r = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*r + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp]) >> 3;
               g = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*g + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 1]) >> 2;
               b = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*b + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 2]) >> 3;
               vram16[offset] = (b << 11)|(g << 5)|r;
               break;
            case 1:// 16-bit RGBA 5:5:5:1
               color = vram16[offset];
               r = (color & 0x1f) << 3;
               g = ((color >> 5) & 0x1f) << 3;
               b = ((color >> 10) & 0x1f) << 3;
               r = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*r + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp]) >> 3;
               g = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*g + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 1]) >> 3;
               b = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*b + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 2]) >> 3;
               vram16[offset] = (b << 10)|(g << 5)|r;
               break;
            case 2:// 16-bit RGBA 4:4:4:4
               color = vram16[offset];
               r = (color & 0xf) << 4;
               g = ((color >> 4) & 0xf) << 4;
               b = ((color >> 8) & 0xf) << 4;
               r = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*r + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp]) >> 4;
               g = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*g + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 1]) >> 4;
               b = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*b + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 2]) >> 4;
               vram16[offset] = (b << 8)|(g << 4)|r;
               break;
            case 3:// 32-bit RGBA 8:8:8:8
               color = vram32[offset];
               r = color & 0xff;
               g = (color >> 8) & 0xff;
               b = (color >> 16) & 0xff;
               r = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*r + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp]);
               g = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*g + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 1]);
               b = (unsigned char)(((255.0f - image[pos*sbpp + 3])/255.0f)*b + (image[pos*sbpp + 3]/255.0f)*image[pos*sbpp + 2]);
               vram32[offset] = (b << 16)|(g << 8)|r;
               break;
         }
         pos++;
      }
   }
   
   sceKernelDcacheWritebackAll();
}
int main_thread(SceSize args, void *argp) {

   sceKernelDelayThread(3000000);//wait for xmb to load up//

 

while(1)
{
	sceCtrlPeekBufferPositive(&pad, 1); // update the var inside the loop 
 if(pad.Buttons & PSP_CTRL_LTRIGGER) drawimage(ourImage, 32, 32, 20, 20);
   sceDisplayWaitVblankStart();
   sceKernelDelayThread(100);
}
   sceKernelSleepThread();
return 0;
}

int module_start(SceSize args, void *argp) {

   int thid;

   /* Create a high priority thread */
   thid = sceKernelCreateThread("driver", main_thread, 0x18, 0x1000, 0, NULL);//8, 64*1024, PSP_THREAD_ATTR_USER, NULL);
   if(thid >= 0) sceKernelStartThread(thid, args, argp);

   return 0;
}