/**
 * Copyright (c) 2022 Brian Starkey <stark3y@gmail.com>
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include <stdio.h>

#define DEBUG_MSG 1

#include "hardware/i2c.h"
#include "pico/stdio.h"
#include "pico/stdlib.h"

#include "camera/camera.h"
#include "camera/format.h"

void camera_lcd_init(uint16_t x, uint16_t y, uint16_t width, uint16_t height);
void camera_lcd_display(uint16_t x, uint16_t y, uint16_t width, uint16_t height, uint16_t *data);

#define CAMERA_PIO      pio0
#define CAMERA_BASE_PIN 16
#define CAMERA_XCLK_PIN 21
#define CAMERA_SDA      6       // original 0
#define CAMERA_SCL      7       // original 1

#define LCD_SX  8
#define LCD_SY  0

static inline int __i2c_write_blocking(void *i2c_handle, uint8_t addr, const uint8_t *src, size_t len)
{
	return i2c_write_blocking((i2c_inst_t *)i2c_handle, addr, src, len, false);
}

static inline int __i2c_read_blocking(void *i2c_handle, uint8_t addr, uint8_t *dst, size_t len)
{
	return i2c_read_blocking((i2c_inst_t *)i2c_handle, addr, dst, len, false);
}

// From http://www.paulbourke.net/dataformats/asciiart/
const char charmap[] = " .:-=+*#%@";
static i2c_inst_t *i2c_inst = i2c1;

void image_terminal_print(struct camera_buffer *buf, uint16_t width, uint16_t height) {
    int y, x;
    for (y = 0; y < height; y++) {
        char row[width];
        for (x = 0; x < width; x++) {
            uint8_t v = buf->data[0][buf->strides[0] * y + x];
            v /= 26;
            row[x] = charmap[v];
        }
        printf("%s\n", row);
    }
}

static uint8_t lcd_buf[CAMERA_WIDTH_DIV8 * CAMERA_HEIGHT_DIV8 * 2];
static const uint16_t width = CAMERA_WIDTH_DIV8;
static const uint16_t height = CAMERA_HEIGHT_DIV8;
static struct camera camera;
static struct camera_buffer *buf = (struct camera_buffer *)NULL;
struct camera_platform_config platform = {
    .i2c_write_blocking = __i2c_write_blocking,
    .i2c_read_blocking = __i2c_read_blocking,
    .i2c_handle = i2c1,

    .pio = CAMERA_PIO,
    .xclk_pin = CAMERA_XCLK_PIN,
    .xclk_divider = 9,
    .base_pin = CAMERA_BASE_PIN,
    .base_dma_channel = -1,
};

void image_lcd_mono_display(struct camera_buffer *buf, uint16_t width, uint16_t height) {
    int y, x;
    uint8_t l, h;
    int t = 0;
    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {
            uint8_t v = buf->data[0][buf->strides[0] * y + x];
            l  = (v & 0xf8) + (v >> 5);
            h  = ((v & 0xfc) << 3) + (v >> 3);
            lcd_buf[t + 0] = l;
            lcd_buf[t + 1] = h;
            t += 2;
        }
    }
    camera_lcd_display(LCD_SX, LCD_SY, CAMERA_WIDTH_DIV8, CAMERA_HEIGHT_DIV8, (uint16_t *)&lcd_buf);
}

int main() {
    stdio_init_all();

    // Wait some time for USB serial connection
    sleep_ms(3000);

    i2c_init(i2c_inst, 100000);     // changed to ch1 from ch0
    gpio_set_function(CAMERA_SDA, GPIO_FUNC_I2C);
    gpio_set_function(CAMERA_SCL, GPIO_FUNC_I2C);
    gpio_pull_up(CAMERA_SDA);
    gpio_pull_up(CAMERA_SCL);
    buf = camera_buffer_alloc(FORMAT_YUV422, width, height);
    int ret = camera_init(&camera, &platform, buf);
    if (ret) {
        printf("camera_init failed: %d\n", ret);
        return 1;
    }
    // assert(buf);
    camera_lcd_init(LCD_SX, LCD_SY, width, height);

    while (1) {
#if DEBUG_MSG
        printf("Capturing...\n");
#endif
        ret = camera_capture_blocking(&camera, buf, true);
        if (ret != 0) {
#if DEBUG_MSG
            printf("Capture error: %d\n", ret);
#endif
        } else {
#if DEBUG_MSG
            printf("Capture success\n");
#endif
            // image_terminal_print(buf, (uint16_t)width, (uint16_t)height);
            image_lcd_mono_display(buf, (uint16_t)width, (uint16_t)height);
        }
        sleep_ms(1);
    }
}
