#include <limits.h>
#include <stdint.h>
#include <stdio.h>
#include <malloc.h>
#include <math.h>
#include <SDL/SDL.h>

#define SECONDARY_SORTING_SPEED 500000
#define width 640
#define height 512

typedef unsigned int uint;

SDL_Surface *sdl_scr;
uint *scr, *zbf;
float *data, tmp[4];
const uint widthheight = width*height;
const int halfwidth = width/2;
const uint halfheight = height/2;
static uint seed=1000;
float randf() {seed=214013*seed+2531011; return (seed>>16)&32767;}

void SWP(uint A, uint B) {
    uint as;
    for(as=0; as<4; as++){
        tmp[as]=data[A+as];
        data[A+as]=data[B+as];
        data[B+as]=tmp[as];
    }
}

uint load_points(const char* filename) {
    FILE *f = fopen(filename, "rb");
    fseek(f, 0, SEEK_END);
    const uint count = (ftell(f)/9);
    fseek(f, 0, SEEK_SET);
    data = (float*)calloc(count, 4*4);
    uint i = 0;
    while (!feof(f)) {
        unsigned short xyz[4];
        unsigned char rgb[3];
        fread(&xyz, 3, 2, f);
        fread(&rgb, 3, 1, f);
        data[i+0] = xyz[0]+randf()/32767.0; // small random offset for de-overlapping points
        data[i+1] = xyz[1]+randf()/32767.0; // (undoes 16bit truncation)
        data[i+2] = xyz[2]+randf()/32767.0; // and turns them into floats.
        data[i+3] = (rgb[0]<<16)|(rgb[1]<<8)|rgb[2];
        i+=4;
    }
    fclose(f);

    // FIRST SHUFFLING
    for (i = 0; i < count*4; i+=4) {
        uint ri = (int(randf()*randf())%count)*4;
        SWP(i, ri);
    }
    return count;
}

__inline void rot2d(float sf, float cf, float *x, float *y) {
    float tx = (*x) * cf - (*y) * sf;
    *y = (*y) * cf + (*x) * sf;
    *x = tx;
}

int main(int argc, char **argv) {
    // PREPARATIONS (MEMORY ALLOCATION, POINT CLOUD DATA LOADING):
    if (argc==1) return 0;
    const uint count = load_points(argv[1]);

    SDL_Init(SDL_INIT_VIDEO);
    //freopen("CON", "w", stdout); // redirects stdout
    //freopen("CON", "w", stderr); // redirects stderr

    sdl_scr = SDL_SetVideoMode(width, height, 32, SDL_HWSURFACE);
    scr = (uint*)calloc(width * height, 4);
    zbf = (uint*)calloc(width * height, 4);
    float camx = 512, camy = 512, camz = 512, yaw = 0, pitch = 0;
    float yx = 0, dx = 0, dy = 0, dz = 0, px = 0;

    uint stack = 0, lasti = 0, limit = count, f = 0, i4 = 0, i = 0, currentcluster = 0;


    // MAIN LOOP:
    for(;;) {
        // 1. CAMERA MOTION
        SDL_PumpEvents();
        static uint lt = 0; float d = SDL_GetTicks() - lt; lt += d; float dt = d<1?1:d;
        if (dt<16) { SDL_Delay(16 - dt); dt = 16; }
        unsigned char *key = SDL_GetKeyState(NULL);
        if (key[SDLK_ESCAPE]) return 0;
        yx = -(key[SDLK_LEFT] - key[SDLK_RIGHT]) * dt/600.0;
        px = (key[SDLK_UP] - key[SDLK_DOWN]) * dt/600.0;
        dx = (key[SDLK_d] - key[SDLK_a]) * dt/4.0;
        dy = (key[SDLK_w] - key[SDLK_s]) * dt/4.0;
        dz = (key[SDLK_SPACE] - key[SDLK_LCTRL]) * dt/4.0;
        yaw += yx; pitch += px;
        float sf = sinf(yaw), cf = cosf(yaw), psf = sinf(pitch), pcf = cosf(pitch);
        float tdx = dx, tdy = dy; rot2d(sf, cf, &tdy, &tdx);
        float speed = key[SDLK_LSHIFT] * dt/10.0+1;
        camx += tdx*speed; camy += tdy*speed; camz += dz*speed;

        // 2. CLEARING SCREEN AND ZBUFFER
        for (i = 0; i < widthheight; i++) { scr[i] = 0; zbf[i] = INT_MAX; }

        // 3. ATOMS DRAWING AND SORTING
        f = 0;

        for (i = 0; i < limit; i++) {
            if (i == stack) {
                i = lasti;
                limit = i + SECONDARY_SORTING_SPEED;
                if (limit > count) limit = count;
            }
            i4 = i*4;
            float x = data[i4 + 0] - camx, y = data[i4 + 1] - camy, z = data[i4 + 2] - camz;
            rot2d(sf, cf, &x, &y); rot2d(psf, pcf, &z, &y);
            if (y <= 0) continue;
            const uint sx = halfwidth  + (x*halfwidth) / y;
            if (sx < 0 || sx >= width) continue;
            const uint sy = halfheight - (z*halfwidth) / y;
            if (sy < 0 || sy >= height) continue;
            const uint sind = sx + sy * width;
            if (zbf[sind] < y) continue;
            scr[sind] = data[i4 + 3];
            zbf[sind] = y;
            SWP(i4, f);
            f += 4;
        }
        stack = f*0.275; // Adaptive stack size
        lasti = i;
        if (lasti >= count) { currentcluster = 0; lasti = stack; } else { currentcluster++; }

        // 4. FLIPPING SCREEN, SHOWING FPS
        if(SDL_MUSTLOCK(sdl_scr)) if(SDL_LockSurface(sdl_scr)<0) return 0;
        memcpy(sdl_scr->pixels, scr, widthheight*4);
        if(SDL_MUSTLOCK(sdl_scr)) SDL_UnlockSurface(sdl_scr);
        SDL_Flip(sdl_scr);
        char buf[256]; sprintf(buf, "FPS:%.2f Stack:%2d Current zone:%d", 1000/dt, stack, currentcluster ); SDL_WM_SetCaption(buf, NULL);
    }
}