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

#include <stdio.h>

#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

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);
}

static i2c_inst_t *i2c_inst = i2c1;
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(80, 80, CAMERA_WIDTH_DIV8, CAMERA_HEIGHT_DIV8, (uint16_t *)&lcd_buf);
}

void move_image(uint8_t *buffer) {
    int y, x;
    uint8_t yy, uu, vv;
    int t = 0;
    for (y = 0; y < height; y++) {
        for (x = 0; x < width; x++) {
            yy = buf->data[0][buf->strides[0] * y + x];
            uu = buf->data[1][buf->strides[1] * y + x/2];
            vv = buf->data[2][buf->strides[2] * y + x/2];
            buffer[t + 0] = yy;
            if ((x & 1) == 0) {
                buffer[t + 1] = uu;
            } else {
                buffer[t + 1] = vv;
            }
            t += 2;
        }
    }
}

int camera_task_init(void) {
    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) {
        return 1;
    }
    // assert(buf);
    camera_lcd_init(80, 80, width, height);
    return 0;
}

void camera_task(void) {
    int ret = camera_capture_blocking(&camera, buf, false);
    // if (ret == 0) {
    // image_lcd_mono_display(buf, (uint16_t)width, (uint16_t)height);
    // }
}
