// INCLUDE HEADERS ///////////////////////////////////////////////////////

// See header file for licensing info
#include "pwc.h"

#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <fcntl.h>
#include <unistd.h>
#include <SDL.h>

// GLOBAL VARS ///////////////////////////////////////////////////////////

int res;
int pan_min = -1;
int pan_max = -1;
int tilt_min = -1;
int tilt_max = -1;
struct video_picture vp;
struct video_window vw;
struct pwc_whitebalance wb;
struct pwc_mpt_range mr;
struct pwc_mpt_angles ma;

// The following are for the SDL viewer
int paused = 0;
int size, fps, i;
unsigned char *y, *u, *v;
int y_data_size, uv_data_size;
SDL_Surface *screen;
SDL_Overlay *overlay;
// SDL_Event event;
SDL_Rect rect = { 0 };
SDL_TimerID timer;

// FUNCTIONS /////////////////////////////////////////////////////////////

// Brightness, contrast, gamma, color, saturation
int camera_brightness_set(int fd, int brightness) {
    if (brightness > MAXPERC) {
        brightness = MAXPERC;
    }
    if (brightness < MINPERC) {
        brightness = MINPERC;
    }
    brightness = brightness * PWC_MAXVAL / MAXPERC;
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    vp.brightness = brightness;
    return ioctl(fd, VIDIOCSPICT, &vp);
}

int camera_brightness(int fd) {
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    return vp.brightness * MAXPERC / PWC_MAXVAL;
}

int camera_contrast_set(int fd, int contrast) {
    if (contrast > MAXPERC) {
        contrast = MAXPERC;
    }
    if (contrast < MINPERC) {
        contrast = MINPERC;
    }
    contrast = contrast * PWC_MAXVAL / MAXPERC;
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    vp.contrast = contrast;
    return ioctl(fd, VIDIOCSPICT, &vp);
}

int camera_contrast(int fd) {
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    return vp.contrast * MAXPERC / PWC_MAXVAL;
}

int camera_gamma_set(int fd, int gamma) {
    if (gamma > MAXPERC) {
        gamma = MAXPERC;
    }
    if (gamma < MINPERC) {
        gamma = MINPERC;
    }
    gamma = gamma * PWC_MAXVAL / MAXPERC;
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    vp.whiteness = gamma;
    return ioctl(fd, VIDIOCSPICT, &vp);
}

int camera_gamma(int fd) {
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    return vp.whiteness * MAXPERC / PWC_MAXVAL;
}

int camera_color_set(int fd, int color) {
    if (color > MAXPERC) {
        color = MAXPERC;
    }
    if (color < MINPERC) {
        color = MINPERC;
    }
    color = PWC_MAXVAL * color / MAXPERC;
    res = ioctl(fd, VIDIOCPWCGAWB, &wb);
    if (res < 0) {
        return -1;
    }
    if (wb.mode != PWC_WB_MANUAL) {
        wb.mode = PWC_WB_MANUAL;
    }
    wb.manual_red = color;
    wb.manual_blue = color;
    return ioctl(fd, VIDIOCPWCSAWB, &wb);
}

int camera_color(int fd) {
    int r = 0;
    int b = 0;
    res = ioctl(fd, VIDIOCPWCGAWB, &wb);
    if (res < 0) {
        return -1;
    }
    if (wb.mode != PWC_WB_MANUAL) {
        r = wb.read_red;
        b = wb.read_blue;
    }
    else {
        r = wb.manual_red;
        b = wb.manual_blue;
    }
    if (r < b) {
        r = b;
    }
    return r * MAXPERC / PWC_MAXVAL;
}

int camera_saturation_set(int fd, int saturation) {
    if (saturation > MAXPERC) {
        saturation = MAXPERC;
    }
    if (saturation < MINPERC) {
        saturation = MINPERC;
    }
    saturation = saturation * PWC_MAXVAL / MAXPERC;
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    vp.colour = saturation;
    return ioctl(fd, VIDIOCSPICT, &vp);
}

int camera_saturation(int fd) {
    res = ioctl(fd, VIDIOCGPICT, &vp);
    if (res < 0) {
        return -1;
    }
    return vp.colour * MAXPERC / PWC_MAXVAL;
}

// Framerate and size
int camera_framerate_set(int fd, int fps) {
    if (fps < PWC_MINFPS) {
        fps = PWC_MINFPS;
    }
    if (fps > PWC_MAXFPS) {
        fps = PWC_MAXFPS;
    }
    res = ioctl(fd, VIDIOCGWIN, &vw);
    if (res < 0) {
        return -1;
    }
    vw.flags = fps << PWC_FPS_SHIFT;
    return ioctl(fd, VIDIOCSWIN, &vw);
}

int camera_framerate(int fd) {
    res = ioctl(fd, VIDIOCGWIN, &vw);
    if (res < 0) {
        return -1;
    }
    return vw.flags >> PWC_FPS_SHIFT;
}

int camera_capsize_set(int fd, int capsize) {
    // Maybe use REALSIZE?
    if (capsize < 0) {
        capsize = 0;
    }
    if (capsize > 5) {
        capsize = 5;
    }
    res = ioctl(fd, VIDIOCGWIN, &vw);
    if (capsize == 0) {
        vw.width = 128;
        vw.height = 96;
    }
    if (capsize == 1) {
        vw.width = 160;
        vw.height = 120;
    }
    if (capsize == 2) {
        vw.width = 176;
        vw.height = 144;
    }
    // Webcam mode is 320x240 (as with the TV drivers). This may be a mistake.
    if (capsize == 3) {
        vw.width = 320;
        vw.height = 240;
    }
    if (capsize == 4) {
        vw.width = 352;
        vw.height = 288;
    }
    if (capsize == 5) {
        vw.width = 640;
        vw.height = 480;
    }
    return ioctl(fd, VIDIOCSWIN, &vw);
}

int camera_capsize(int fd) {
    // Maybe use REALSIZE?
    res = ioctl(fd, VIDIOCGWIN, &vw);
    if (res < 0) {
        return -1;
    }
    if (vw.width == 128 && vw.height == 96) return 0;
    if (vw.width == 160 && vw.height == 120) return 1;
    if (vw.width == 176 && vw.height == 144) return 2;
    if (vw.width == 320 && vw.height == 240) return 3;
    if (vw.width == 352 && vw.height == 288) return 4;
    if (vw.width == 640 && vw.height == 480) return 5;
    return -1;
}

// MPT stuff
int camera_panning_set(int fd, int pan) {
    if (pan > MAXPERC) {
        pan = MAXPERC;
    }
    if (pan < MINPERC) {
        pan = MINPERC;
    }
    if (pan_min == -1 || pan_max == -1) {
        res = ioctl(fd, VIDIOCPWCMPTGRANGE, &mr);
        if (res < 0) {
            return -1;
        }
        pan_min = mr.pan_min;
        pan_max = mr.pan_max;
    }
    // can't assume symmetry, e.g. min = -max
    // min is negative, and actual pan = 0 is at center, that much is certain
    if (pan < MAXPERC/2) {
        pan = pan_min - 2 * pan * pan_min / MAXPERC;
    }
    else {
        pan = 2 * pan * pan_max / MAXPERC - pan_max;
    }
    res = ioctl(fd, VIDIOCPWCMPTGANGLE, &ma);
    if (res < 0) {
        return -1;
    }
    ma.absolute = 1;
    ma.pan = pan;
    return ioctl(fd, VIDIOCPWCMPTSANGLE, &ma);
}
    
int camera_panning(int fd) {
    if (pan_min == -1 || pan_max == -1) {
        res = ioctl(fd, VIDIOCPWCMPTGRANGE, &mr);
        if (res < 0) {
            close(fd);
            return -1;
        }
        pan_min = mr.pan_min;
        pan_max = mr.pan_max;
    }
    res = ioctl(fd, VIDIOCPWCMPTGANGLE, &ma);
    if (res < 0) {
        return -1;
    }
    if (ma.pan < 0) {
        return MAXPERC / 2 - MAXPERC * ma.pan / pan_min / 2;
    }
    else {
        return MAXPERC / 2 * (ma.pan + pan_max) / pan_max;
    }
}

int camera_tilting_set(int fd, int tilt) {
    if (tilt > MAXPERC) {
        tilt = MAXPERC;
    }
    if (tilt < MINPERC) {
        tilt = MINPERC;
    }
    if (tilt_min == -1 || tilt_max == -1) {
        res = ioctl(fd, VIDIOCPWCMPTGRANGE, &mr);
        if (res < 0) {
            return -1;
        }
        tilt_min = mr.tilt_min;
        tilt_max = mr.tilt_max;
    }
    if (tilt < MAXPERC/2) {
        tilt = tilt_min - 2 * tilt * tilt_min / MAXPERC;
    }
    else {
        tilt = 2 * tilt * tilt_max / MAXPERC - tilt_max;
    }
    res = ioctl(fd, VIDIOCPWCMPTGANGLE, &ma);
    if (res < 0) {
        return -1;
    }
    ma.absolute = 1;
    ma.tilt = tilt;
    return ioctl(fd, VIDIOCPWCMPTSANGLE, &ma);
}

int camera_tilting(int fd) {
    if (tilt_min == -1 || tilt_max == -1) {
        res = ioctl(fd, VIDIOCPWCMPTGRANGE, &mr);
        if (res < 0) {
            return -1;
        }
        tilt_min = mr.tilt_min;
        tilt_max = mr.tilt_max;
    }
    res = ioctl(fd, VIDIOCPWCMPTGANGLE, &ma);
    if (res < 0) {
        return -1;
    }
    if (ma.tilt < 0) {
        return MAXPERC / 2 - MAXPERC * ma.tilt / tilt_min / 2;
    }
    else {
        return MAXPERC/2 * (ma.tilt + tilt_max) / tilt_max;
    }
}

// VIEWER FUNCTIONS //////////////////////////////////////////////////////

int viewer_init(int fd, int width, int height) {
    // Capture size vw.width, vw.height
    if (ioctl(fd, VIDIOCGWIN, &vw) < 0) return 1;
    size = vw.width * vw.height * 3/2; // YUV12 not YUV2
    
    // Framerate
    fps = vw.flags >> PWC_FPS_SHIFT;
    
    // Pointers to pixel data YV12 (YYYYVU)
    if ((y = malloc(size)) == NULL) return 2;
    y_data_size = vw.width * vw.height;
    uv_data_size = y_data_size / 4;
    v = y + y_data_size;  // vw.width * vw.height is size of the Ys
    u = v + uv_data_size; // 1/4th is size of the Vs

    // SDL init and such
    rect.w = width;
    rect.h = height;

    if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER) < 0) {
        SDL_Quit();
        return 3;
    }

    if ((screen = SDL_SetVideoMode(rect.w, rect.h, 0, SDL_RESIZABLE)) == NULL) {
        SDL_Quit();
        return 4;
    }

    if ((overlay = SDL_CreateYUVOverlay(vw.width, vw.height, SDL_YV12_OVERLAY, screen)) == NULL) {
        SDL_Quit();
        return 5;
    }

    SDL_DisplayYUVOverlay(overlay, &rect);
    paused = 1;

    // Inspired by pwcview: set a timer in SDL based on framerate setting
    if ((timer = SDL_AddTimer(((1000/fps)/10)*10, timer_callback, NULL)) == NULL) {
        SDL_Quit();
        return 6;
    }

    // We need to be able to refer to the open file descriptor (ugly)
    i = fd;

    // All succeeded
    return 0;
}

void viewer_start() {
    paused = 0;
}

void viewer_pause() {
    paused = 1;
}
    
void viewer_resize(int width, int height) {
    rect.w = width;
    rect.h = height;
    SDL_SetVideoMode(rect.w, rect.h, 0, SDL_RESIZABLE);
    if (paused) {
        SDL_DisplayYUVOverlay(overlay, &rect);
    }
}

void viewer_quit() {
    free(y);
    SDL_Quit();
}

void frame_handler() {
    if (!paused) {
        // Read image data
        if (read(i, y, size) == size) {
            // Update
            SDL_LockYUVOverlay(overlay);
            memcpy(overlay->pixels[0], y, y_data_size);
            memcpy(overlay->pixels[1], u, uv_data_size);
            memcpy(overlay->pixels[2], v, uv_data_size);
            SDL_UnlockYUVOverlay(overlay);
            SDL_DisplayYUVOverlay(overlay, &rect);
        }
    }
}

unsigned int timer_callback(unsigned int interval, void *param) {
    frame_handler();
    return interval;
}

// END ///////////////////////////////////////////////////////////////////