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

#define SECONDARY_SORTING_SPEED 300000
#define width 1024
#define height 768

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

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

int load_points(const char* filename) {
    FILE *f = fopen(filename, "rb");
    fseek(f, 0, SEEK_END);
    const int count = (ftell(f)/9);
    fseek(f, 0, SEEK_SET);
    data = (int*)calloc(count, 4*4);
    int 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];
        data[i+1] = xyz[2];
        data[i+2] = xyz[1];
        data[i+3] = (rgb[0]<<16)|(rgb[1]<<8)|rgb[2];
        i+=4;
    }
    fclose(f);
// FIRST SORTING. IT IS SIMPLE, BUT VERY IMPORTANT STAGE FOR THIS ALGORITHM.
    for (i = 0; i < count*4; i+=4) {
        int ri = ((_rand()*_rand())%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 int count = load_points(argv[1]);
    sdl_scr = SDL_SetVideoMode(width, height, 32, SDL_HWSURFACE);
    scr = (int*)calloc(width * height, 4);
    zbf = (int*)calloc(width * height, 4);
    float camx = 512, camy = 512, camz = 512, yaw = 0;
    float yx = 0, dx = 0, dy = 0, dz = 0;

    int thresh = 0, lasti = 0, limit = count, f = 0, i4 = 0, i = 0;


    // MAIN LOOP:
    for(;;) {
        // 1. CAMERA MOTION
        SDL_PumpEvents();
        static unsigned 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/500.0;
        dx += (key[SDLK_d] - key[SDLK_a]) * dt/48.0;
        dy += (key[SDLK_SPACE] - key[SDLK_LCTRL]) * dt/48.0;
        dz += (key[SDLK_w] - key[SDLK_s]) * dt/48.0;
        yaw += yx; float sf = sinf(yaw), cf = cosf(yaw);
        float tdz = dz, tdx = dx; rot2d(sf, cf, &tdz, &tdx);
        camx += tdx; camy += dy; camz += tdz;
        float dimv = (1.0 - dt/500.0); if (dimv<0) dimv = 0; if (dimv>1) dimv = 1;
        dx = dx * dimv; dy = dy * dimv; dz = dz * dimv;
        if (fabs(dx)<0.1) dx = 0; if (fabs(dy)<0.1) dy = 0; if (fabs(dz)<0.1) dz = 0;

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

        // 3. ATOMS DRAWING ALONG WITH _SORTING_
        f = 0;
        for (i = 0; i < limit; i++) {
            if (i == thresh) {
                i = lasti;
                limit = i + SECONDARY_SORTING_SPEED;
                if (limit > count) limit = count;
            }
            i4 = i*4;
            int x = data[i4 + 0] - camx, y = data[i4 + 1] - camy, z = data[i4 + 2] - camz;
            float fx = x, fz = z; rot2d(sf, cf, &fx, &fz); x = fx; z = fz;
            if (z <= 0) continue;
            const int sx = halfwidth  + (x*halfwidth) / z;
            if (sx < 0 || sx >= width) continue;
            const int sy = halfheight - (y*halfwidth) / z;
            if (sy < 0 || sy >= height) continue;
            const int sind = sx + sy * width;
            if (zbf[sind] <= z+1) continue;
            scr[sind] = data[i4 + 3];// if (f > thresh) scr[sind] = 0xff;
            zbf[sind] = z;
            if (f < thresh) SWP(i4, f*4);
            f++;
        }
        thresh = f + 1000; // Adaptive threshold
        lasti = i;
        if (lasti >= count) lasti = thresh;

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