#include <Arduino_FreeRTOS.h>

//LCD Display Pins
#define LCD_EN 1 << DDA1;//22
#define LCD_RS 1 << DDA0;//23
#define LCD_D4 1 << DDA2;//24
#define LCD_D5 1 << DDA3;//25
#define LCD_D6 1 << DDA4;//26
#define LCD_D7 1 << DDA5;//27

//Initializes LCD
void initLCD(){
  // Pin setup
  DDRA |= LCD_EN;
  DDRA |= LCD_RS;
  DDRA |= LCD_D4;
  DDRA |= LCD_D5;
  DDRA |= LCD_D6;
  DDRA |= LCD_D7;

  delay(50);
  // Display setup
  // commandLCD(0, 0x02); // 4-bit mode
  // commandLCD(0, 0x28); // 4-bit mode / 2 lines / 5 x 8 matrix
  // commandLCD(0, 0x0C); // display on / cursor off
  // commandLCD(0, 0x06); // increment cursor
  // commandLCD(0, 0x01); // clear screen
  // commandLCD(0, 0x80); // cursor set to home

  // Initialize LCD in 4-bit mode
  commandLCD(0, 0x03);
  delay(5);
  commandLCD(0, 0x03);
  delayMicroseconds(100);
  commandLCD(0, 0x03);
  delay(5);
  commandLCD(0, 0x02);

  // Function Set: 4-bit mode, 2 lines, 5x8 font
  commandLCD(0, 0x28);

  // Display On/Off: Display on, Cursor off, Blink off
  commandLCD(0, 0x0C);

  // Clear Display
  commandLCD(0, 0x01);
  delay(2);

}

//Sets the digital pins for the LCD
void setLCDBit(uint8_t pin, int bit){
  if (bit == 1){
    PORTA |= (pin);
  }else{
    PORTA &= ~(pin);
  }
}

//Sends Commands to LCD
void commandLCD(unsigned char rs, unsigned char data){
  // Set register select pin
  if(rs == 1){
    PORTA |= LCD_RS; // command
  }else{
    PORTA &= !LCD_RS;  // write
  }

  // Send higher nibble (bits 4-7)
  PORTA |= LCD_EN; // set enable pin

  if (((data >> 4) & 0x01) == 1){
    PORTA |= LCD_D4;
  }else{
    PORTA &= !LCD_D4;
  }

  if (((data >> 5) & 0x01) == 1){
    PORTA |= LCD_D5;
  }else{
    PORTA &= !LCD_D5;
  }

  if (((data >> 6) & 0x01) == 1){
    PORTA |= LCD_D6;
  }else{
    PORTA &= !LCD_D6;
  }

  if (((data >> 7) & 0x01) == 1){
    PORTA |= LCD_D7;
  }else{
    PORTA &= !LCD_D7;
  }

  // setLCDBit(LCD_D4, int((data >> 4) & 0x01));
  // setLCDBit(LCD_D5, int((data >> 5) & 0x01));
  // setLCDBit(LCD_D6, int((data >> 6) & 0x01));
  // setLCDBit(LCD_D7, int((data >> 7) & 0x01));
  PORTA &= !LCD_EN; // set enable pin
  delayMicroseconds(1);

  // Send lower nibble (bits 0-3)
  PORTA |= LCD_EN; // set enable pin

  if (((data >> 0) & 0x01) == 1){
    PORTA |= LCD_D4;
  }else{
    PORTA &= !LCD_D4;
  }

  if (((data >> 1) & 0x01) == 1){
    PORTA |= LCD_D5;
  }else{
    PORTA &= !LCD_D5;
  }

  if (((data >> 2) & 0x01) == 1){
    PORTA |= LCD_D6;
  }else{
    PORTA &= !LCD_D6;
  }

  if (((data >> 3) & 0x01) == 1){
    PORTA |= LCD_D7;
  }else{
    PORTA &= !LCD_D7;
  }

  // setLCDBit(LCD_D4, int((data >> 0) & 0x01));
  // setLCDBit(LCD_D5, int((data >> 1) & 0x01));
  // setLCDBit(LCD_D6, int((data >> 2) & 0x01));
  // setLCDBit(LCD_D7, int((data >> 3) & 0x01));
  PORTA &= !LCD_EN; // set enable pin
  delayMicroseconds(100);
}

//Writes a string to the LCD
void writeLCD(const char *str){
  while(*str){
    commandLCD(1, *str++);
  }
}

void setup() {
  // Initialize LCD
  initLCD();
}

void loop() {
  commandLCD(0, 0x80); // Set cursor to the beginning of the first line
  writeLCD("Hello, LCD!");
  delay(2000);
  commandLCD(0, 0x01); // Clear display
  delay(1000);
}