/*
 * Copyright (C) 2019  Max Regan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <cstring>

#include "DisplayDriver.h"
#include "macros.h"
#include "font.h"

namespace BSP {

using Common::Schedule::NextTime;
using Common::ReturnCode;

DisplayDriver::DisplayDriver(Common::Schedule::TaskScheduler &scheduler, SpiDriver &spi)
    : m_scheduler(scheduler)
    , m_spi(spi)
    , m_is_dirty(true)
    , m_dirty_line_min(0)
    , m_dirty_line_max(0)
{
    buffer_init();
}

ReturnCode DisplayDriver::init()
{
    return Common::ReturnCode::OK;
}

NextTime DisplayDriver::execute()
{
    return NextTime::never();
}

void DisplayDriver::buffer_init()
{
    for (size_t i = 0; i < ARRAY_SIZE(m_buffer.lines); i++) {
        struct display_line &line = m_buffer.lines[i];
        line.mode = 1; // Update display
        line.line = i + 1; // Line numbers start at 1
        for (size_t j = 0; j < ARRAY_SIZE(line.data); j++) {
            line.data[j] = 0xFF;
        }
    }

    m_buffer.dummy = 0;
}

void DisplayDriver::set_dirty(unsigned int y)
{
    if (!m_is_dirty) {
        m_is_dirty = true;
        m_dirty_line_min = y;
        m_dirty_line_max = y;
    } else {
        m_dirty_line_min = MIN(y, m_dirty_line_min);
        m_dirty_line_max = MAX(y, m_dirty_line_max);
    }
}

void DisplayDriver::set_bit(uint32_t x, uint32_t y, uint8_t val)
{
    if (x >= DISPLAY_WIDTH || y >= DISPLAY_HEIGHT) {
        return;
    }

    struct display_line &line = m_buffer.lines[y];
    uint8_t *byte = &line.data[x >> 3];
    if (val) {
        CLR_POS(*byte, x & 7);
    } else {
        SET_POS(*byte, x & 7);
    }

    set_dirty(y);
}

void DisplayDriver::set_byte(uint32_t x, uint32_t y, uint8_t val)
{
    // if (x >= DISPLAY_WIDTH || y >= DISPLAY_HEIGHT) {
    //     return;
    // }

    // if (x & 7) {
    //     return;
    // }

    struct display_line &line = m_buffer.lines[y];
    line.data[x >> 3] = val;
    set_dirty(y);
}


// TODO: write my own implementation
#define R2(n)     n,     n + 2*64,     n + 1*64,     n + 3*64
#define R4(n) R2(n), R2(n + 2*16), R2(n + 1*16), R2(n + 3*16)
#define R6(n) R4(n), R4(n + 2*4 ), R4(n + 1*4 ), R4(n + 3*4 )

static const unsigned char BitReverseTable256[256] =
{
    R6(0), R6(2), R6(1), R6(3)
};

unsigned char ReverseBitsLookupTable(unsigned char v)
{
    return BitReverseTable256[v];
}

/**
 *  This variant is ~4x faster than the unaligned version, but
 *  (obviously) requires that everything is aligned correctly.
 */

void DisplayDriver::clear_glyph_aligned(uint32_t x_off, uint32_t y_off, const struct font *, const struct glyph *g)
{
    for (uint32_t y = y_off; y < y_off + g->rows && y < DISPLAY_HEIGHT; y++) {
        uint8_t *start = (uint8_t *) &m_buffer.lines[y].data[(x_off) >> 3];
        uint8_t *end = (uint8_t *) &m_buffer.lines[y].data[(x_off + g->advance) >> 3];
        memset(start, 0xFF, end - start);
    }
}

// void DisplayDriver::bit_copy(uint8_t *src, unsigned int src_bit_offset,
//                              uint8_t *dst, unsigned int dst_bit_offset,
//                              unsigned int bit_len)
// {
//     uint8_t buffer;

//     if (src_bit_offset == && dst_bit_offset == 0) {
//         /* The "happy" case, where both src and dst are byte-aligned */
//         unsigned int byte_count = bit_len / 8;
//         memcpy(dst, src, byte_count);
//         if (bit_len & 7) {
//             uint8_t mask = (1 << bit_len & 7) - 1;
//             dst[byte_count] &= ~mask;
//             dst[byte_count] |=  mask & src[byte_count];
//         }
//         return;
//     }

//     if (bit_len >= 8) {

//         // Start the initial byte
//         buffer =  *(src++) >> src_bit_offset;
//         buffer |= *(src) << (8 - src_bit_offset);


//         // The main copy loop


//         // Set the last byte/bits

//     }

//     for (bits_copied = 0; bits_copied + 8 < bit_len; bits_copied += 8) {
//         *dst
//     }

// }

void DisplayDriver::clear_glyph_unaligned(uint32_t x_off, uint32_t y_off, const struct font *font, const struct glyph *g)
{
    int16_t x = 0;
    if (x & 7) {
        while ((x & 7) && x < g->advance) {
            // TODO: use a switch on (x & 7) instead?
            for (int16_t y = 0; y < g->rows && y < (int16_t) DISPLAY_HEIGHT; y++) {
                set_bit(x_off + x, y_off + y + font->size - g->top, 0);
            }
            x++;
        }
    }

    while (g->advance - x > 0) {
        for (int16_t y = 0; y < g->rows && y < (int16_t) DISPLAY_HEIGHT; y++) {
            set_bit(x_off + x, y_off + y + font->size - g->top, 0);
        }
        x++;
    }

}


void DisplayDriver::write_glyph_unaligned(uint32_t x_off, uint32_t y_off, const struct font *font, const struct glyph *g)
{
    int byte_cols = g->cols / 8;
    if (g->cols & 7) {
        byte_cols++;
    }

    for (size_t x = 0; x < g->cols; x++) {
        for (size_t y = 0; y < g->rows && y < DISPLAY_HEIGHT; y++) {
            int byte_x = x / 8;
            int byte_y = y;
            uint8_t bit = (g->bitmap[byte_y * byte_cols + byte_x] >> (7 - (x & 7))) & 1;
            set_bit(g->left + x_off + x,
                    y_off + y + font->size - g->top,
                    bit);
        }
    }

}

// void DisplayDriver::write_glyph_unaligned2(int *x_off, int y_off, const struct font *font, const struct glyph *g)
// {
//     int byte_cols = g->cols / 8;
//     if (g->cols & 7) {
//         byte_cols++;
//     }

//     for (size_t x = 0; x < g->cols; x++) {
//         for (size_t y = 0; y < g->rows && y < DISPLAY_HEIGHT; y++) {
//             int byte_x = x / 8;
//             int byte_y = y;
//             uint8_t bit = (g->bitmap[byte_y * byte_cols + byte_x] >> (7 - (x & 7))) & 1;
//             set_bit(g->left + *x_off + x, y_off + y + font->size - g->top, bit);
//         }
//     }

// }

/**
 *  This variant is ~4x faster than the unaligned version, but
 *  requires that everything is aligned correctly.
 */
void DisplayDriver::write_glyph_aligned(uint32_t x_off, uint32_t y_off,
                                        const struct font *font, const struct glyph *g)
{
    int byte_cols = g->cols / 8;
    if (g->cols & 7) {
        byte_cols++;
    }


    for (size_t x = 0; x < g->cols; x += 8) {
        for (size_t y = 0, byte_y = 0; y < g->rows && y < DISPLAY_HEIGHT; y++, byte_y += byte_cols) {
            int byte_x = x / 8;
            uint8_t byte = g->bitmap[byte_y + byte_x];
            set_byte(g->left + x_off + x,
                     y_off + y + font->size - g->top,
                     ~ReverseBitsLookupTable(byte));
        }
    }
}

void DisplayDriver::char_at(uint32_t *x_off, uint32_t y_off, char c, const struct font *font)
{
    const struct glyph *g = glyph_for_char(font, c);
    if (g == NULL) {
        return;
    }

    if (*x_off + g->left + g->cols > DISPLAY_WIDTH) {
        return;
    }

    if (!(*x_off & 7) && !((*x_off + g->advance) & 7)) {
        clear_glyph_aligned(*x_off, y_off, font, g);
    } else {
        clear_glyph_unaligned(*x_off, y_off, font, g);
    }

    // FIXME: REALLY DO THIS!
    // Check the right glyph boundary (g->left + g->cols)
    if (!((*x_off + g->left) & 7)) {
        write_glyph_aligned(*x_off, y_off, font, g);
    } else {
        write_glyph_unaligned(*x_off, y_off, font, g);
    }

    m_dirty_line_min = MIN(m_dirty_line_min, y_off);
    m_dirty_line_max = MAX(m_dirty_line_max, y_off + g->rows);
    m_is_dirty = true;
    *x_off += g->advance;
}

void DisplayDriver::string_at(uint32_t *x_off, uint32_t y_off, const char *string, const struct font *font)
{
    int i = 0;
    while (string[i]) {
        char_at(x_off, y_off, string[i], font);
        i++;
    }
}

// TODO: Implement this
// void DisplayDriver::rect_at(int x_off, int y_off,
//                             int width, int height,
//                             bool is_black,
//                             int line_width)
// {

// }

void DisplayDriver::refresh()
{
    if (!m_is_dirty) {
        return;
    }

    uint8_t *start = (uint8_t *) &m_buffer.lines[m_dirty_line_min];
    // Data size
    size_t size = sizeof(m_buffer.lines[0]) * (m_dirty_line_max - m_dirty_line_min + 1);
    // Trailer dummy data
    size += 2;

    m_spi.tx_blocking(start, size);
    m_is_dirty = false;
    m_dirty_line_min = DISPLAY_HEIGHT - 1;
    m_dirty_line_max = 0;
}

void DisplayDriver::clear()
{
    buffer_init();
    m_is_dirty = true;
    m_dirty_line_min = 0;
    m_dirty_line_max = DISPLAY_HEIGHT - 1;
}

//TODO: put me somewhere fonty
const struct glyph *DisplayDriver::glyph_for_char(const struct font *font, char c)
{
    std::size_t low = 0;
    std::size_t high = font->count - 1;

    while (low <= high) {
        std::size_t mid = (high - low) / 2;
        mid += low;

        const struct glyph *g = font->glyphs[mid];
        if (g->glyph == c) {
            return g;
        }

        if (g->glyph > c) {
            high = mid - 1;
        } else {
            low = mid + 1;
        }
    }
    return NULL;
}

}