#include <stdint.h>
#include <stdio.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <termios.h>
#include <signal.h>
#include <stdarg.h>
#include <stdlib.h>

volatile sig_atomic_t flag = 0;

void Error(const char * format, ...) {
    fprintf(stderr, "Error: ");
    va_list args;
    va_start(args, format);
    vfprintf(stderr, format, args);
    va_end(args);
    fprintf(stderr, "\n");
}

// fast and only approximate
void DownsampleFrameBufferBGRA2RGB(uint8_t *in, int xin, int yin, uint8_t* out, int xout, int yout){
    int sx = xin/xout;
    int sy = yin/yout;
    int size = sx*sy;
    for(int y = 0; y != yout; y++){
        for(int x = 0; x != xout; x++){
            int xmin = x*xin/xout;
            int ymin = y*yin/yout;
            int xmax = xmin+sx;
            int ymax = ymin+sy;
            
            int r = 0, g = 0, b = 0;
            int idx = xmin+ymin*xin;
            uint8_t *mem = in+4*idx;
            for (int yp = ymin; yp != ymax; yp++ ){
                for (int xp =xmin; xp != xmax; xp++ ){
                    b += *mem++;
                    g += *mem++;
                    r += *mem++;
                    mem++;
                }
                mem += 4*(xin-sx);
            }
            *out++ = r/size;
            *out++ = g/size;
            *out++ = b/size;
        }
    }
}



void SignalHandler(int sig){
    flag = 1;
}

int main(int argc, char **argv) {
    //DISPMANX_RESOURCE_HANDLE_T screen_resource;
    uint32_t                   image_prt;
    //VC_RECT_T                  rect1;
    int ret;
    int fbfd = 0;
    char *framebuffer_gbra = 0;

    struct fb_var_screeninfo vinfo;
    struct fb_fix_screeninfo finfo;
    int serial;
    int x = atoi(argv[2]);
    int y = atoi(argv[3]);
    uint8_t * out_rgb = malloc(x*y*3+1);

    signal(SIGINT, SignalHandler); 

    if (argc < 4){
        fprintf(stderr, "Wrong number of parameters, should be:\n\n");
        fprintf(stderr, " fbcp <framebuffer> <sizex> <sizey>\n\n");
        fprintf(stderr, "i.e. fbcp /dev/fb0\n");
        goto exit;
    }


    fbfd = open(argv[1], O_RDWR);
    if (fbfd == -1) {
        Error("Unable to open display %s", argv[2]);
        goto clean1_1;
    }
    if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {
        Error("Unable to get display information");
        goto clean2;
    }
    if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo)) {
        Error("Unable to get display information");
        goto clean2;
    }

    fprintf(stderr, "Display is %d x %d %dbps\n", vinfo.xres, vinfo.yres, vinfo.bits_per_pixel);

    if (vinfo.bits_per_pixel != 32){
        Error("Display has %i bits per pixel. This program only works for 32.", vinfo.bits_per_pixel);
    }

    framebuffer_gbra = (char*) mmap(0, finfo.smem_len, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, 0);
    if (framebuffer_gbra <= 0) {
        Error("Unable to create framebuffer memory mapping");
        goto clean3;
    }

    FILE *logfile = fopen("out.log", "w");
    int frame = 0;
    while(!flag) {

       // vc_dispmanx_resource_read_data(screen_resource, &rect1, framebuffer_gbra, vinfo.xres * vinfo.bits_per_pixel / 8);
        fflush(logfile);
        uint8_t* out = out_rgb;
        *out++ = 1; // Begin of frame marker
        DownsampleFrameBufferBGRA2RGB(framebuffer_gbra, vinfo.xres, vinfo.yres, out, x, y);
        write(1, out_rgb+1, x*y*3);

        frame++;
    }
    
clean4:
    munmap(framebuffer_gbra, finfo.smem_len);
clean3:
clean2:
    close(fbfd);
clean1_1:
clean1:
exit:
    return 0;
}