#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

#include "ssd1306.h"

static void ssd1306_display(const struct driver *drv);
static void ssd1306_cls(const struct driver *drv);
static void ssd1306_dot(const struct driver *drv, uint32_t x, uint32_t y);
static void ssd1306_line(const struct driver *drv, int16_t x0, int16_t y0, int16_t x1, int16_t y1);
static void ssd1306_circle(const struct driver *drv, int16_t x0, int16_t y0, int16_t radius);

static void ssd1306_on(const struct driver *drv);
static void ssd1306_off(const struct driver *drv);
static void ssd1306_cmd(const struct driver *drv, uint8_t c);
static void ssd1306_reset_cursor(const struct driver *drv);

int ssd1306_open(const struct driver *drv)
{
    assert(drv != NULL);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    drv_open(this->output_dev);

    ssd1306_off(drv);

    ssd1306_cmd(drv, 0xa8); // Set Multiplex Ratio
    ssd1306_cmd(drv, 0x3f); // Value
    ssd1306_cmd(drv, 0xd3); // Set Display offset
    ssd1306_cmd(drv, 0x00); // no vertical shift
    ssd1306_cmd(drv, 0x40); // set display start line
    ssd1306_cmd(drv, 0xa1); // set segment re-map
    ssd1306_cmd(drv, 0xc8);
    ssd1306_cmd(drv, 0xda);
    ssd1306_cmd(drv, 0x12);
    ssd1306_cmd(drv, 0xa4);
    ssd1306_cmd(drv, 0xa6);
    ssd1306_cmd(drv, 0xd5);
    ssd1306_cmd(drv, 0x80);
    ssd1306_cmd(drv, 0x8d);
    ssd1306_cmd(drv, 0x14);
    ssd1306_cmd(drv, 0x20);
    ssd1306_cmd(drv, 0x20);

    ssd1306_on(drv);
    ssd1306_reset_cursor(drv);

    return 0;
}

int ssd1306_close(const struct driver *drv)
{
    assert(drv != 0);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    ssd1306_off(drv);

    drv_close(this->output_dev);

    return 0;
}

int ssd1306_ioctl(const struct driver *drv, unsigned int cmd, unsigned int argc, va_list args)
{
    assert(drv != 0);

    int res = 0;
            unsigned int xy[4];

    switch(cmd) {
        case IOCTL_CMD_CLS:
            ssd1306_cls(drv);
            break;
        case IOCTL_CMD_REFRESH:
            ssd1306_display(drv);
            break;
        case IOCTL_CMD_DRAW_PIXEL:
            if(argc != 2) {
                res = -1;
                break;
            }
            for(unsigned int i = 0; i < argc; i++) {
                xy[i] = va_arg(args, unsigned int);
            }
            ssd1306_dot(drv, xy[0], xy[1]);
            break;
        case IOCTL_CMD_DRAW_LINE:
            if(argc != 4) {
                res = -1;
                break;
            }
            for(unsigned int i = 0; i < argc; i++) {
                xy[i] = va_arg(args, unsigned int);
            }
            ssd1306_line(drv, xy[0], xy[1], xy[2], xy[3]);
            break;
        case IOCTL_CMD_DRAW_CIRCLE:
            if(argc != 3) {
                res = -1;
                break;
            }
            for(unsigned int i = 0; i < argc; i++) {
                xy[i] = va_arg(args, unsigned int);
            }
            ssd1306_circle(drv, xy[0], xy[1], xy[2]);
            break;
    }

    return res;
}

static void ssd1306_cmd(const struct driver *drv, uint8_t c)
{
    assert(drv != NULL);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    char control[] = {0x00, c};   // Co = 0, D/C = 0
    drv_write(this->output_dev, control, sizeof(control));
}

static void ssd1306_off(const struct driver *drv)
{
    assert(drv != NULL);

    ssd1306_cmd(drv, 0xae); // display off
}

static void ssd1306_on(const struct driver *drv)
{
    assert(drv != NULL);
    ssd1306_cmd(drv, 0xaf); // display on
}

static void ssd1306_reset_cursor(const struct driver *drv)
{
    assert(drv != NULL);
    ssd1306_cmd(drv, 0x21); // # set column address
    ssd1306_cmd(drv, 0x00); // #   set start address
    ssd1306_cmd(drv, 0x7F); // #   set end address (127 max)
    ssd1306_cmd(drv, 0x22); // # set page address
    ssd1306_cmd(drv, 0x00); // #   set start address
    ssd1306_cmd(drv, 0x07); // #   set end address (7 max)
}

static void ssd1306_cls(const struct driver *drv)
{
    assert(drv != NULL);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    memset(this->bitmap, 0x00, (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8));
}

static void ssd1306_display(const struct driver *drv)
{
    assert(drv != NULL);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    drv_write(this->output_dev, this->output_buffer, (SSD1306_LCDWIDTH * SSD1306_LCDHEIGHT / 8) + 1);
}

static void ssd1306_dot(const struct driver *drv, uint32_t x, uint32_t y)
{
    assert(drv != NULL);

    struct ssd1306 *this = (struct ssd1306 *)(drv->dev);

    this->bitmap[x + (y / 8) * SSD1306_LCDWIDTH] |=  (1 << (y & 7));
}

#ifndef _swap_int16_t
#define _swap_int16_t(a, b) { int16_t t = a; a = b; b = t; }
#endif

static void ssd1306_line(const struct driver *drv, int16_t x0, int16_t y0, int16_t x1, int16_t y1)
{
    assert(drv != NULL);

    int16_t steep = abs(y1 - y0) > abs(x1 - x0);
    if(steep) {
        _swap_int16_t(x0, y0);
        _swap_int16_t(x1, y1);
    }
    if(x0 > x1) {
        _swap_int16_t(x0, x1);
        _swap_int16_t(y0, y1);
    }

    int16_t dx, dy;
    dx = x1 - x0;
    dy = abs(y1 - y0);

    int16_t err = dx / 2;
    int16_t ystep;

    if(y0 < y1) {
        ystep = 1;
    } else {
        ystep = -1;
    }

    for(; x0 <= x1; x0++) {
        if(steep) {
            ssd1306_dot(drv, y0, x0);
        } else {
            ssd1306_dot(drv, x0, y0);
        }
        err -= dy;
        if (err < 0) {
            y0 += ystep;
            err += dx;
        }
    }
}

static void ssd1306_circle(const struct driver *drv, int16_t x0, int16_t y0, int16_t radius)
{
    assert(drv != NULL);

    int16_t x = 0, y = radius;
    int16_t dp = 1 - radius;
    do {
        if (dp < 0) {
            dp = dp + (x++) * 2 + 3;
        } else {
            dp = dp + (x++) * 2 - (y--) * 2 + 5;
        }

        ssd1306_dot(drv, x0 + x, y0 + y);     //For the 8 octants
        ssd1306_dot(drv, x0 - x, y0 + y);
        ssd1306_dot(drv, x0 + x, y0 - y);
        ssd1306_dot(drv, x0 - x, y0 - y);
        ssd1306_dot(drv, x0 + y, y0 + x);
        ssd1306_dot(drv, x0 - y, y0 + x);
        ssd1306_dot(drv, x0 + y, y0 - x);
        ssd1306_dot(drv, x0 - y, y0 - x);
    } while (x < y);

    ssd1306_dot(drv, x0 + radius, y0);
    ssd1306_dot(drv, x0, y0 + radius);
    ssd1306_dot(drv, x0 - radius, y0);
    ssd1306_dot(drv, x0, y0 - radius);
}