---

#include <zephyr/kernel.h>
#include <zephyr/drivers/gpio.h>

#define MSG_SIZE 17
#define QUEUE_MAX_MSGS 10
//#define LCD_THREAD_STACK 1024
#define LCD_THREAD_STACK 2048
#define LCD_THREAD_PRIO 14

static uint8_t cur_col = 0;
/* Get PCF8574 from dts */
static const struct device *pcf_dev = DEVICE_DT_GET(DT_NODELABEL(pcf8574));

#define LCD_RS        (1 << 0) // Port 0
#define LCD_RW        (1 << 1) // Port 1
#define LCD_E         (1 << 2) // Port 2
#define LCD_BL        (1 << 3) // Port 3 (Backlight)
#define LCD_DATA_MASK 0xF0     // Port 4-7 (D4-D7)

#define LCD_GPIO_PORT_RAW

/* Global var for lcd backlight */
static uint8_t backlight_state = LCD_BL;

//LCD Message 
struct lcd_msg_t {
    char text[MSG_SIZE];
    uint8_t row;
    uint8_t col;
    uint8_t mode;
};
K_MSGQ_DEFINE(lcd_msgq, sizeof(struct lcd_msg_t), QUEUE_MAX_MSGS, 4);
K_MUTEX_DEFINE(lcd_i2c_mutex);

/* Send 4 bit data*/
void lcd_send_nibble(uint8_t nibble, uint8_t rs_flag)
{
    // Set byte: 4 bits - data, 4 bits- backlight and control
    uint8_t port_val = (nibble << 4) | rs_flag | backlight_state;
    
    // RW always 0 (write mode)
    port_val &= ~LCD_RW; 

    // 1. Set data and RS, E -> 0
#ifdef LCD_GPIO_PORT_RAW
    gpio_port_set_masked_raw(pcf_dev, 0xFF, port_val);
#else
    gpio_port_set_bits_raw(pcf_dev, port_val);
    gpio_port_clear_bits_raw(pcf_dev, ~port_val & 0xFF);
#endif
    k_busy_wait(1);

    // 2. Enable (E = 1) for set data on display
#ifdef LCD_GPIO_PORT_RAW
    gpio_port_set_masked_raw(pcf_dev, 0xFF, port_val | LCD_E);
#else
    gpio_port_set_bits_raw(pcf_dev, LCD_E);
#endif
    k_busy_wait(1);

    // 3. Reset Enable (E = 0)
#ifdef LCD_GPIO_PORT_RAW
    gpio_port_set_masked_raw(pcf_dev, 0xFF, port_val);
#else
    gpio_port_clear_bits_raw(pcf_dev, LCD_E);
#endif
    k_busy_wait(50);
}

/* Send 8 bit function */
void lcd_send_byte(uint8_t value, uint8_t rs_flag)
{
    if(k_mutex_lock(&lcd_i2c_mutex, K_MSEC(100)) == 0) {
        lcd_send_nibble(value >> 4, rs_flag);
        lcd_send_nibble(value & 0x0F, rs_flag);
        k_mutex_unlock(&lcd_i2c_mutex);
    }
}

void lcd_command(uint8_t cmd)  { lcd_send_byte(cmd, 0); }
void lcd_data(uint8_t data)   { lcd_send_byte(data, LCD_RS); }

/* Init display 1602A in 4-bit mode */
int lcd_init(void)
{
    int i,ret;

    if(!device_is_ready(pcf_dev)) {
        printf("Error: No i2c PCF device\n");
        return -ENODEV;
    }
    for(i=0; i < 8; i++)
    {
        ret = gpio_pin_configure(pcf_dev, i, GPIO_OUTPUT_INACTIVE);
        if(ret < 0)
        {
            printf("Error gpio_pin_configure %d err=%d\n",i,ret);
            //return ret;
        }
    }
    k_msleep(50);
    // Set controller HD44780 to 4-bit mode
    lcd_send_nibble(0x03, 0);
    k_msleep(5);
    lcd_send_nibble(0x03, 0);
    k_busy_wait(150);
    lcd_send_nibble(0x03, 0);
    k_busy_wait(150);
    lcd_send_nibble(0x02, 0);
    k_msleep(2);

    // Set display parameters
    lcd_command(0x28); // 4-bit mode, 2 lines, font 5x8
    lcd_command(0x0C); // Display ON, Cursor OFF, Blinking OFF
    lcd_command(0x06); // Autoincrement cursor
    lcd_command(0x01); // Clean display
    k_msleep(2);

    return 1;
}

/* Set cursor position */
void lcd_set_cursor(uint8_t col, uint8_t row)
{
    uint8_t row_offsets[] = {0x00, 0x40};
    lcd_command(0x80 | (col + row_offsets[row]));
    cur_col = col;
}

/* Print line */
void lcd_print(const char *str)
{
    uint8_t count = cur_col;
    while (*str && count < 16) {
        lcd_data(*str++);
        count++;
    }
}

//mode = 0 - clear 16 symbols, add data
//mode = 1 - only clear 16 symbols
//mode = 2 - append mode data without clear
void lcd_render_thread(void *p1, void *p2, void *p3)
{
    struct lcd_msg_t msg;
    uint8_t i = 0;

    while (1) {
        // Wait message queue
        if (k_msgq_get(&lcd_msgq, &msg, K_FOREVER) == 0)
        {
            if(msg.mode != 2) //not append mode
            {
                lcd_set_cursor(msg.col, msg.row);
                for(i=0; i < 16 - msg.col; i++)
                    lcd_data(' ');
            }
            if(msg.mode != 1)
            {
                lcd_set_cursor(msg.col, msg.row);
                lcd_print(msg.text);
            }
            k_msleep(50);
        }
    }
}

K_THREAD_DEFINE(lcd_thread_id, LCD_THREAD_STACK, lcd_render_thread, NULL, NULL, NULL, LCD_THREAD_PRIO, 0, 0);

int safe_lcd_print(uint8_t col, uint8_t row, const char *text, uint8_t mode)
{
    struct lcd_msg_t msg;

    msg.col = col;
    msg.row = row;
    msg.mode = mode;
    memset(msg.text, 0, MSG_SIZE);
    strncpy(msg.text, text, MSG_SIZE - 1);
    msg.text[MSG_SIZE - 1] = '\0';

    // Set to queue, if queue is full - don't wait (K_NO_WAIT)
    return k_msgq_put(&lcd_msgq, &msg, K_NO_WAIT);
}

[00:01:47.333,000] <err> dev_i2s_mcux: buffer 0x20040280 -> out_queue 0x80004544 err -35
[00:01:47.731,000] <wrn> dev_i2s_mcux: TX input queue empty!
[00:01:47.731,000] <wrn> dev_i2s_mcux: TX is paused.
[00:02:04.434,000] <err> os: ***** USAGE FAULT *****
[00:02:04.434,000] <err> os:   Illegal load of EXC_RETURN into PC
[00:02:04.434,000] <err> os: r0/a1:  0x009affff  r1/a2:  0x00750000  r2/a3:  0x0093ffff
[00:02:04.434,000] <err> os: r3/a4:  0x008bffff r12/ip:  0x0098ffff r14/lr:  0x00740001
[00:02:04.434,000] <err> os:  xpsr:  0x0076fe00
[00:02:04.434,000] <err> os: s[ 0]:  0x006e0001  s[ 1]:  0x008a0000  s[ 2]:  0x00850000  s[ 3]:  0x00740000
[00:02:04.434,000] <err> os: s[ 4]:  0x006e0000  s[ 5]:  0x00950000  s[ 6]:  0x0085ffff  s[ 7]:  0x00900000
[00:02:04.434,000] <err> os: s[ 8]:  0x00820001  s[ 9]:  0x00940000  s[10]:  0x00a30001  s[11]:  0x0083ffff
[00:02:04.434,000] <err> os: s[12]:  0x006d0000  s[13]:  0x007a0000  s[14]:  0x00840000  s[15]:  0x008c0001
[00:02:04.434,000] <err> os: fpscr:  0x007a0000
[00:02:04.434,000] <err> os: Faulting instruction address (r15/pc): 0x00830000
[00:02:04.434,000] <err> os: >>> ZEPHYR FATAL ERROR 34: Unknown error on CPU 0
[00:02:04.434,000] <err> os: Current thread: 0x80008600 (lcd_thread_id)
[00:02:04.444,000] <err> os: Halting system