#include <bcm2835.h>
#include <stdio.h>
#include <sys/mman.h>
#include <fcntl.h>

#include <time.h>
#include <string.h>
#include <unistd.h>

// соответствия контактов GPIO и LCD
#define LCD_RS RPI_V2_GPIO_P1_11
#define LCD_RESET RPI_V2_GPIO_P1_13

#define RS_CMD 0
#define RS_DATA 1

#define CMD_RESET 0x01
#define CMD_MEMORY_ACCESS_CONTROL 0x36 // Memory Access Control
#define CMD_WAKEUP 0x11 // Выход из спящего режима
#define CMD_PALETTE 0x3A // Установка цветовой палитры
#define CMD_ENABLE 0x29  //Включение дисплея

#define CMD_SET_X 0x2A // задаем область по X
#define CMD_SET_Y 0x2B // задаем область по Y
#define CMD_START_WRITE 0x2C // начало записи в память

#define _8_BIT_COLOR 0x02
#define _12_BIT_COLOR 0x03
#define _16_BIT_COLOR 0x05

#define SCREEN_WIDTH 132
#define SCREEN_HEIGHT 176

int init_gpio()
{
    if (!bcm2835_init())
        return 0;

    bcm2835_spi_begin();
    bcm2835_spi_setBitOrder(BCM2835_SPI_BIT_ORDER_MSBFIRST);
    bcm2835_spi_setDataMode(BCM2835_SPI_MODE0);                 /// CPOL = 0, CPHA = 0, Clock idle low,
                                                                /// data is clocked in on rising edge,
                                                                /// output data (change) on falling edge
    bcm2835_spi_setClockDivider(BCM2835_SPI_CLOCK_DIVIDER_16);  ///< 16 = 64ns = 15.625MHz
    bcm2835_spi_chipSelect(BCM2835_SPI_CS0);                    /// Select Our Device
    bcm2835_spi_setChipSelectPolarity(BCM2835_SPI_CS0, LOW);

    // настроим порты на запись
    bcm2835_gpio_fsel(LCD_RS, BCM2835_GPIO_FSEL_OUTP);
    bcm2835_gpio_fsel(LCD_RESET, BCM2835_GPIO_FSEL_OUTP);
    
    return 1;
}

int close_gpio()
{
    bcm2835_spi_end();
    bcm2835_close();
}

void send_cmd(char cmd)
{
    bcm2835_gpio_write(LCD_RS, RS_CMD);
    bcm2835_spi_transfer(cmd);
}

void send_data(char data)
{
    bcm2835_gpio_write(LCD_RS, RS_DATA);
    bcm2835_spi_transfer(data);
}

void send_data_array(char* data, int size)
{
    bcm2835_gpio_write(LCD_RS, RS_DATA);
    bcm2835_spi_transfern(data, size);
}

void set_draw_area(char x1, char y1, char x2, char y2)
{
    send_cmd(CMD_SET_X);
    send_data(x1);
    send_data(x2);

    send_cmd(CMD_SET_Y);
    send_data(y1);
    send_data(y2);
}

void draw_start()
{
    send_cmd(CMD_START_WRITE);
    bcm2835_gpio_write(LCD_RS, RS_DATA);
}

void send_draw_data(char* data, int size)
{
    bcm2835_spi_transfern(data, size);
}

void reset_LCD()
{
    bcm2835_gpio_write(LCD_RESET, LOW);
    bcm2835_delay(50);
    bcm2835_gpio_write(LCD_RESET, HIGH);
    bcm2835_delay(50);

    send_cmd(CMD_RESET);
}

void init_LCD()
{
    reset_LCD();

    send_cmd(CMD_MEMORY_ACCESS_CONTROL);
    send_data(0b00000000);

    send_cmd(CMD_WAKEUP);

    bcm2835_delay(20);

    send_cmd(CMD_PALETTE);
    send_data(_16_BIT_COLOR);

    bcm2835_delay(20);

    send_cmd(CMD_ENABLE);
}

#define FB_WIDTH 320// 176
#define FB_HEIGHT 240// 144

int main(int argc, char **argv)
{
    if (!init_gpio())
        return 1;

    int smooth = 0;
    int dynamic_fps = 0;
    int argn = 1;
    while(argn < argc)
    {
        if(argv[argn][0] == '-')
            switch(argv[argn][1])
            {
            case 'i':
                init_LCD();
                close_gpio();
                printf("lcd initialized\n");
                return 0;
                break;

            case 's':
                smooth = 1;
                break;
            case 'd':
                dynamic_fps = 1;
                break;
            default:
                printf("Usage: lcd [options]\n");
                printf("Options:\n");
                printf("  -i   initialize lcd (hardware reset)\n");
                printf("  -d   dynamic FPS (skip same frames)\n");
                printf("  -s   smooth image (enable basic intrpolation)\n");
                return 0;
                break;
            }
        argn++;
    }

    ///------------------------------------------------
    /// draw screen

    set_draw_area(0, 0, SCREEN_WIDTH - 1, SCREEN_HEIGHT - 1);
    draw_start();
    
    uint16_t screen[SCREEN_HEIGHT][SCREEN_WIDTH];

    uint16_t old_fb[FB_HEIGHT * FB_WIDTH];

    int fd_scr = open("/dev/fb0", O_RDONLY);
    int scr_sz = FB_HEIGHT * FB_WIDTH * 2/*16bit*/;
    uint16_t* fb_screenshot = mmap(0, scr_sz, PROT_READ, MAP_PRIVATE, fd_scr, 0);

    // scaling
    float scale_X = FB_HEIGHT / (float)SCREEN_WIDTH;
    float scale_Y = FB_WIDTH / (float)SCREEN_HEIGHT;

    int frame_cnt = 0;
    struct timespec ts1, ts2;
    clock_gettime(CLOCK_MONOTONIC, &ts1);

    for(;;) // forever
    {
        if(dynamic_fps)
            if(memcmp(&old_fb, fb_screenshot, sizeof(old_fb)) == 0)
            {
                usleep(10000);
                continue;
            }
            else
            {
                memcpy(&old_fb, fb_screenshot, sizeof(old_fb));
            }

        for(int x = 0; x < SCREEN_WIDTH; x++)
            for(int y = 0; y < SCREEN_HEIGHT; y++)
            {
                int fb_x = y * scale_X;
                int fb_y = x * scale_Y;

                uint16_t px = fb_screenshot[fb_x + fb_y * FB_WIDTH];

                if(smooth)
                {
                    // look around
                    if((fb_x - 1 >= 0) && (fb_x + 1 < FB_WIDTH) && (fb_y - 1 >= 0) && (fb_y + 1 < FB_HEIGHT))
                    {
                        for(int dx = -1; dx <= 1; dx++)
                            for(int dy = -1; dy <= 1; dy++)
                                if((dx == 0) ^ (dy == 0))
                                {
                                    uint16_t add_px = fb_screenshot[(fb_x + dx) + (fb_y + dy) * FB_WIDTH];
                                    px = ((px & 0xf7de) >> 1) + ((add_px & 0xf7de) >> 1);
                                    /// ^thank you habr => http://habr.ru/p/128773/
                                }
                    }
                }
                screen[y][SCREEN_WIDTH - 1 - x] = (px << 8) | (px >> 8);
            }

        send_draw_data((char*)&screen[0][0], sizeof(screen));

        /// calc fps
        frame_cnt++;
        if(frame_cnt >= 100)
        {
            clock_gettime(CLOCK_MONOTONIC, &ts2);

            float allsec = (ts2.tv_sec - ts1.tv_sec) + (ts2.tv_nsec - ts1.tv_nsec) / 1000000000.0;
            float fps = frame_cnt / allsec;

            printf("%f FPS\n", fps);

            frame_cnt = 0;
            clock_gettime(CLOCK_MONOTONIC, &ts1);
        }

        usleep(1000);
    }

    munmap(fb_screenshot, scr_sz);
    close(fd_scr);
    close_gpio();

    printf("fin\n");    
    return 0;
}