#include <avr/io.h>
#include <avr/delay.h>
#include <string.h>
#include "display.h"

void displaySetNibbleH(unsigned char data) {
    if(data & 0x80) PORTA.OUT |= DISPLAY_D7; else PORTA.OUT &= ~DISPLAY_D7;
    if(data & 0x40) PORTA.OUT |= DISPLAY_D6; else PORTA.OUT &= ~DISPLAY_D6;
    if(data & 0x20) PORTA.OUT |= DISPLAY_D5; else PORTA.OUT &= ~DISPLAY_D5;
    if(data & 0x10) PORTA.OUT |= DISPLAY_D4; else PORTA.OUT &= ~DISPLAY_D4;
}

void displaySetNibbleL(unsigned char data) {
    if(data & 0x08) PORTA.OUT |= DISPLAY_D7; else PORTA.OUT &= ~DISPLAY_D7;
    if(data & 0x04) PORTA.OUT |= DISPLAY_D6; else PORTA.OUT &= ~DISPLAY_D6;
    if(data & 0x02) PORTA.OUT |= DISPLAY_D5; else PORTA.OUT &= ~DISPLAY_D5;
    if(data & 0x01) PORTA.OUT |= DISPLAY_D4; else PORTA.OUT &= ~DISPLAY_D4;
}

void displayToggleEnable() { 
    PORTA.OUT |= DISPLAY_E; // Set Enable pin high
    _delay_us(10);
    PORTA.OUT &= ~DISPLAY_E; // Set Enable pin low
    _delay_us(40);
}

void displayBacklightOn() {
    PORTA.OUT |= DISPLAY_BACKLIGHT;
}

void displayBacklightOff() {
    PORTA.OUT &= ~DISPLAY_BACKLIGHT;
}

void displayCommandByte(unsigned char data) {
    unsigned char nibbleH = ((data & 0xF0) >> 4);   // mask the higher bits and shift down
    unsigned char nibbleL =  (data & 0x0F);         // mask the lower bits

    PORTA.OUT &= ~DISPLAY_RS; // Set RS pin low (command mode)

    displaySetNibbleH(data);
    displayToggleEnable();
    displaySetNibbleL(data);
    displayToggleEnable();
}

void displayDataByte(unsigned char data) {
    unsigned char nibbleH = ((data & 0xF0) >> 4);   // mask the higher bits and shift down
    unsigned char nibbleL =  (data & 0x0F);         // mask the lower bits

    PORTA.OUT |= DISPLAY_RS; // Set RS pin high (data mode)

    displaySetNibbleH(data);
    displayToggleEnable();
    displaySetNibbleL(data);
    displayToggleEnable();

    PORTA.OUT &= ~DISPLAY_RS; // Set RS pin high (data mode)

}

void displayInit() {
    // Set the IO pins to output
    PORTA.DIRSET |= DISPLAY_BACKLIGHT |
                    DISPLAY_E |
                    DISPLAY_RS |
                    DISPLAY_D4 |
                    DISPLAY_D5 |
                    DISPLAY_D6 |
                    DISPLAY_D7;

    // Run the reset sequence
    _delay_ms(5);          
    PORTA.OUT &= ~DISPLAY_RS; // Set RS pin low (command mode)

    displaySetNibbleL(0x03);
    displayToggleEnable();
    _delay_ms(5);
    displaySetNibbleL(0x03);
    displayToggleEnable();
    _delay_ms(5);
    displaySetNibbleL(0x03);
    displayToggleEnable();
    _delay_ms(5);
    displaySetNibbleL(0x02);
    displayToggleEnable();
    _delay_ms(5);
    displayCommandByte(0x2C);
    displayCommandByte(0x0C);
    displayCommandByte(0x01);
    displayCommandByte(0x06);
}

void displayClear() {
    displayCommandByte(0x01);
    displayCommandByte(0x00);
}

void displayWriteLine(unsigned char x, unsigned char y, unsigned char *str) {
    // move cursor to first position of specified line
    // 0x80 (0b10000000) is first character top line
    // 0xc0 (0b11000000) is first character bottom line
    displayCommandByte((0x80 | (x & 0x0f) | (y & 0x01) << 6) );
    for(size_t i = 0; i < strlen(str); i++) {
        displayDataByte(str[i]);
    }
}