#include <Arduino.h>
#include <osd.h>
#include <Wire.h>
#include <WireSlave.h>
#include "config.h"
static const char TAG[] = __FILE__;

extern TaskHandle_t OSD_Task;
extern t_osd_info osd_info;
extern struct osd_display_t osd_display;
extern bool masterboot;
namespace gotek {
    byte RxByte;
    byte RxByte2;
    uint8_t osd_buttons_rx;

    void OSD_Start(void * parameter){

        while(1){
        vTaskDelay(1000);
        Serial.println("!");
        }
    }

    void OSD_RxHandler(int numBytes)
    {
        
        bool command_end=false;
        uint x=0;
        uint y=0;
        while (Wire.available())
        { // Read Any Received Data
            RxByte = Wire.read();
            if (!command_end){
                if ((RxByte & 0xc0) == OSD_COLUMNS) {
                    /* 0-40 */
                    osd_display.cols = (RxByte & 0x3f);
                } else {
                    switch (RxByte & 0xf0) {
                    case OSD_BUTTONS:
                        osd_buttons_rx=(RxByte & 0x0f);
                        break;
                    case OSD_ROWS:
                        /* 0-3 */
                        osd_display.rows=(RxByte & 0x03);
                        break;
                    case OSD_HEIGHTS:
                        osd_display.heights= (RxByte & 0x0d);
                        break;
                    case OSD_BACKLIGHT:
                        switch (RxByte & 0x0f) {
                        case 0:
                            osd_display.on=false;
                            break;
                        case 1:
                            osd_display.on=true;
                            break;
                        case 2:
                            command_end=true;
                            break;
                        }
                    }
                }
            }else{
                if (x<osd_display.cols){
                    osd_display.text[y][x++]  = RxByte;
                }else{
                    ESP_LOGV(TAG,"%s",(char *)osd_display.text[y]);//osd_display.text[y]);
                    x=0;
                    y++;
                    osd_display.text[y][x++]  = RxByte;
                }
            }
            
        }
        ESP_LOGV(TAG,"%s",((char *)osd_display.text[y]));

    }
    void OSD_RxRequest()
    {
        uint8_t *info = (uint8_t *)&osd_info;
        if (masterboot)
            Wire.write(info , 4);
        else
            Wire.write(info ,5 );
        ESP_LOGV(TAG,"Button %i",info[3]); //"PER"
        osd_info.buttons=0x00;
        osd_info.buttons2=0x00;
    }
    void OSD_RxHandler2(int numBytes)
    {
        while (Wire1.available())
        { // Read Any Received Data
            RxByte2 = Wire1.read();
        }
    }
    void OSD_RxRequest2()
    {
        uint8_t *info = (uint8_t *)&osd_info;
        Wire1.write(info , 4);
        Wire1.write(0x00);
        Serial.println("YYYYYYYYYYYYYYYYYYYYYYY");
    }
    void osd::start(){
        xTaskCreate(
        OSD_Start,
        "OSD_Task_1",
        2000,
        NULL,
        1,
        &OSD_Task);
    };
    
    osd gotek_osd;
}
    