/*
  wrapper for OSD code (https://github.com/night-ghost/minimosd-extra) to run in the HAL as independent process

    (c) night_ghost@ykoctpa.ru 2017

*/

#include <AP_HAL/AP_HAL.h>

#ifdef BOARD_OSD_CS_PIN

#include <utility>

#include "osd_core/compat.h"



using namespace F4Light;

#include <AP_Common/AP_Common.h>
#include <stdio.h>

#include <hal.h>
#include "ring_buffer.h"

#include <inttypes.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/print_vprintf.h>
#include <AP_HAL_F4Light/AP_HAL_F4Light.h>
#include <AP_HAL_F4Light/SPIDevice.h>

#define SLAVE_BUILD

#include "osd_core/Defs.h"

#include "osd.h"


#include "osd_eeprom.h"
#include "osd_core/eeprom.h"
#include "osd_core/version.h"
#include <sd/SD.h>


namespace OSDns {

class BetterStream : AP_HAL::BetterStream {};

#include "osd_core/GCS_MAVLink.h"

#include "osd_core/OSD_Max7456.h"
OSD osd; //OSD object

#include "osd_core/prototypes.h"
#include "osd_core/Vars.h"



#include "osd_core/Config_Func.h"
#include "osd_core/Config.h"
#include "osd_core/Func.h"
#include "osd_core/protocols.h"
#include "osd_core/misc.h"

#include "osd_core/Params.h"
#include "osd_core/Panels.h"


// TODO: чтение конфига и еепром с карты памяти, чтобы закинуть .mcm и .osd и все       
static const char * const words[] = {
    "Air Speed",        // 1
    "Altitude",         // 2
    "Auto Mode",        // 3
    "Auto Screen Switch", // 4 
    "batt_a_k",         // 5
    "BattB",            // 6
    "batt_b_k",         // 7
    "Battery",          // 8
    "Battery A",        // 9
    "Battery B",        // 10
    "Battery Percent",  // 11
    "Battery Warning Level", // 12
    "Call Sign",        // 13
    "Chanel Rotation Switching", // 14
    "Channel Raw",      // 15
    "Channel Scale",    // 16
    "Channel state",    // 17
    "Channel Value",    // 18

    "Configuration",    // 19
    "Current",          // 20
    "curr_k",           // 21
    "Efficiency",       // 22
    "fBattA",           // 23
    "fBattB",           // 24      
    "fCurr",            // 25
    "fILS",             // 26
    "flgHUD",           // 27
    "flgOnce",          // 28
    "flgTrack",         // 29
    "Flight Data",      // 30
    "Flight Mode",      // 31
    "fRussianHUD",      // 32
    "GPS Coord",        // 33
    "GPS HDOP",         // 34
    "Heading",          // 35
    "Heading Rose",     // 36
    "Home Altitude",    // 37
    "Home Direction",   // 38
    "Home Distance",    // 39
    "Horizon",          // 40
    "HOS",              // 41
    "Message",          // 42
    "Model Type",       // 43
    "NSCREENS",         // 44
    "OSD Brightness",   // 45
    "Overspeed",        // 46

   "Panel",             // 47
    "Pitch",            // 48
    "pitch_k",          // 49
    "pitch_kn",         // 50
    "PWMDST",           // 51
    "PWMSRC",           // 52
    "Radar Scale",      // 53
    "Real heading",     // 54
    "Roll",             // 55
    "roll_k",           // 56
    "roll_kn",          // 57
    "RSSI",             // 58
    "RSSI Enable Raw",  // 59
    "RSSI High",        // 60
    "rssi_k",           // 61
    "RSSI Low",         // 62
    "RSSI Warning Level", // 63
    "SAdd1",            // 64
    "SAdd2",            // 65
    "SAdd3",            // 66
    "SAdd4",            // 67
    "Sensor 1",         // 68
    "Sensor 2",         // 69
    "Sensor 3",         // 70
    "Sensor 4",         // 71
    "SFactor1",         // 72
    "SFactor2",         // 73
    "SFactor3",         // 74
    "SFactor4",         // 75
    "SFormat1",         // 76
    "SFormat2",         // 77
    "SFormat3",         // 78
    "SFormat4",         // 79
    "Stall",            // 80
    "Temperature",      // 81
    "Throttle",         // 82
    "Time",             // 83
    "Toggle Channel",   // 84
    "Trip Distance",    // 85
    "Tune",             // 86
    "txtTime0",         // 87
    "txtTime1",         // 88
    "txtTime2",         // 89
    "txtTime3",         // 90
    "Units",            // 91
    "Velocity",         // 92
    "Vertical Speed",   // 93
    "Video Mode",       // 94
    "Visible Sats",     // 95
    "VOS",              // 96
    "Warnings",         // 97
    "Wind Speed",       // 98
    "WP Direction",     // 99
    "WP Distance",      // 100
    "#",                // 101
    "Power",            // 102
    "Date",             // 103
    "Time of day",      // 104
    "Motors",           // 105
    "Vibrations",       // 106
    "Variometer",       // 107
    "GPS Coord Lat",    // 108
    "GPS Coord Lon",    // 109
};
    
typedef struct OSD_PAN {
    uint8_t dst;
    uint8_t size;
    char fmt;
    uint8_t pan_n;
} OSD_pan;

#define m1 ((uint8_t)-1)

static const OSD_pan pan_tbl[]={
    { 0, 0, 0, 0, },
    { 0, 0, 0, ID_of(airSpeed), },     //    "Air Speed",        // 1
    { 0, 0, 0, ID_of(alt), },          //    "Altitude",         // 2
    { 9,    m1, 0, 0, },               //        "Auto Mode",        // 3 - bit in flags
    { 7,    m1, 0, 0, },               //        "Auto Screen Switch", // 4 
    { offsetof(Settings,evBattA_koef),    sizeof(sets.evBattA_koef),   'f', 0, }, //        "batt_a_k",         
    { offsetof(Settings,battBv),          sizeof(sets.battBv),         'b', 0, },  //        "BattB",            
    { offsetof(Settings,evBattB_koef),    sizeof(sets.evBattB_koef),   'f', 0, }, //        "batt_b_k",         
    { offsetof(Settings,battv),           sizeof(sets.battv),          'b', 0, },  //        "Battery",          
    { 0, 0, 0, ID_of(batt_A), },       //        "Battery A",        // 9
    { 0, 0, 0, ID_of(batt_B), },       //        "Battery B",        // 10
    { 0, 0, 0, ID_of(batteryPercent), },//       "Battery Percent",  // 11
    { offsetof(Settings,batt_warn_level), sizeof(sets.batt_warn_level),'b', 0, },  //        "Battery Warning Level", // 12
    { offsetof(Settings,OSD_CALL_SIGN),   sizeof(sets.OSD_CALL_SIGN),  'c', 0, },  //        "Call Sign",        // 13
    { offsetof(Settings,switch_mode),     sizeof(sets.switch_mode),    'b', 0, },  //        "Chanel Rotation Switching", // 14
    { 0, 0, 0, ID_of(ch), },          //        "Channel Raw",      // 15
    { 0, 0, 0, ID_of(Scale), },       //        "Channel Scale",    // 16
    { 0, 0, 0, ID_of(State), },       //        "Channel state",    // 17
    { 0, 0, 0, ID_of(CValue), },      //        "Channel Value",    // 18

    { 0, 0, 0, 0, },                  //         "Configuration",    // 19
    { 0, 0, 0, ID_of(curr_A), },      //        "Current",          // 20
    { offsetof(Settings,eCurrent_koef),   sizeof(sets.eCurrent_koef),  'f', 0, },      //        "curr_k",           // 21
    { 0, 0, 0, ID_of(eff), },         //        "Efficiency",       // 22
    { 4, m1,   0, 0, },               //        "fBattA",           // 23
    { 5, m1,   0, 0, },               //        "fBattB",           // 24      
    { 6, m1,   0, 0, },               //        "fCurr",            // 25
    { 0, 0,    0, 0, },               // 26
    { 0, 0,    0, 0, },               // 27
    { 0, m1,   0, 0, },               //        "flgOnce",          // 28
    { 0, 0,    0, 0, },         //29
    { 0, 0, 0, ID_of(Fdata), },       //        "Flight Data",      // 30
    { 0, 0, 0, ID_of(FMod), },        //        "Flight Mode",      // 31
    { 0, 0, 0, 0, },                  //        "fRussianHUD",      // 32
    { 0, 0, 0, ID_of(GPS), },         //        "GPS Coord",        // 33
    { 0, 0, 0, ID_of(Hdop), },        //        "GPS HDOP",         // 34
    { 0, 0, 0, ID_of(heading), },     //        "Heading",          // 35
    { 0, 0, 0, ID_of(rose), },        //        "Heading Rose",     // 36
    { 0, 0, 0, ID_of(homeAlt), },     //        "Home Altitude",    // 37
    { 0, 0, 0, ID_of(homeDir), },     //        "Home Direction",   // 38
    { 0, 0, 0, ID_of(homeDist), },    //        "Home Distance",    // 39
    { 0, 0, 0, ID_of(horizon), },     //        "Horizon",          // 40
    { offsetof(Settings,horiz_offs),      sizeof(sets.horiz_offs),    'b', 0, },  //        "HOS",              // 41
    { 0, 0, 0, ID_of(message), },         //        "Message",          // 42
    { offsetof(Settings,model_type),      sizeof(sets.model_type),    'b', 0, },  //        "Model Type",       // 43
    { offsetof(Settings,n_screens),       sizeof(sets.n_screens),     'b', 0, },  //        "NSCREENS",         // 44
    { offsetof(Settings,OSD_BRIGHTNESS),  sizeof(sets.OSD_BRIGHTNESS),'b', 0, },  //        "OSD Brightness",   // 45
    { offsetof(Settings,overspeed),       sizeof(sets.overspeed),     'b', 0, },  //        "Overspeed",        // 46

    { 0, 0, 0, 0, },                      //       "Panel",             // 47
    { 0, 0, 0, ID_of(pitch), },           //        "Pitch",            // 48
    { offsetof(Settings,horiz_kPitch),    sizeof(sets.horiz_kPitch),  'f', 0, }, //        "pitch_k",          // 49
    { offsetof(Settings,horiz_kPitch_a),  sizeof(sets.horiz_kPitch_a),'f', 0, }, //        "pitch_kn",         // 50
    { offsetof(Settings,pwm_dst),         sizeof(sets.pwm_dst),       'b', 0, }, //        "PWMDST",           // 51
    { offsetof(Settings,pwm_src),         sizeof(sets.pwm_src),       'b', 0, }, //        "PWMSRC",           // 52
    { 0, 0, 0, ID_of(RadarScale), },         //        "Radar Scale",      // 53
    { 0, 0, 0, ID_of(COG), },                //        "Real heading",     // 54
    { 0, 0, 0, ID_of(roll), },               //        "Roll",             // 55
    { offsetof(Settings,horiz_kRoll),    sizeof(sets.horiz_kRoll),    'f', 0, }, //        "roll_k",           // 56
    { offsetof(Settings,horiz_kRoll_a),  sizeof(sets.horiz_kRoll_a),  'f', 0, }, //        "roll_kn",          // 57
    { 0, 0, 0, ID_of(RSSI), },               //        "RSSI",             // 58
    { offsetof(Settings,RSSI_raw),       sizeof(sets.RSSI_raw),       'b', 0, }, //        "RSSI Enable Raw",  // 59
    { offsetof(Settings,RSSI_16_high),   sizeof(sets.RSSI_16_high),   'w', 0, }, //        "RSSI High",        // 60
    { offsetof(Settings,eRSSI_koef),     sizeof(sets.eRSSI_koef),     'f', 0, }, //        "rssi_k",           // 61
    { offsetof(Settings,RSSI_16_low),    sizeof(sets.RSSI_16_low),    'w', 0, }, //        "RSSI Low",         // 62
    { offsetof(Settings,rssi_warn_level),sizeof(sets.rssi_warn_level),'b', 0, }, //        "RSSI Warning Level", // 63
    { 0, 0, 0, 0, },         //        "SAdd1",            // 64     // sensors not supported
    { 0, 0, 0, 0, },         //        "SAdd2",            // 65
    { 0, 0, 0, 0, },         //        "SAdd3",            // 66
    { 0, 0, 0, 0, },         //        "SAdd4",            // 67
    { 0, 0, 0, ID_of(sensor1), },         //        "Sensor 1",         // 68
    { 0, 0, 0, ID_of(sensor2), },         //        "Sensor 2",         // 69
    { 0, 0, 0, ID_of(sensor3), },         //        "Sensor 3",         // 70
    { 0, 0, 0, ID_of(sensor4), },         //        "Sensor 4",         // 71
    { 0, 0, 0, 0, },         //        "SFactor1",         // 72
    { 0, 0, 0, 0, },         //        "SFactor2",         // 73
    { 0, 0, 0, 0, },         //        "SFactor3",         // 74
    { 0, 0, 0, 0, },         //        "SFactor4",         // 75
    { 0, 0, 0, 0, },         //        "SFormat1",         // 76
    { 0, 0, 0, 0, },         //        "SFormat2",         // 77
    { 0, 0, 0, 0, },         //        "SFormat3",         // 78
    { 0, 0, 0, 0, },         //        "SFormat4",         // 79
    { offsetof(Settings,stall),         sizeof(sets.stall),           'b', 0, },         //        "Stall",            // 80
    { 0, 0, 0, ID_of(temp), },         //        "Temperature",      // 81
    { 0, 0, 0, ID_of(throttle), },     //        "Throttle",         // 82
    { 0, 0, 0, ID_of(time), },         //        "Time",             // 83
    { offsetof(Settings,ch_toggle),     sizeof(sets.ch_toggle),       'b', 0, },         //        "Toggle Channel",   // 84
    { 0, 0, 0, ID_of(distance), },     //        "Trip Distance",    // 85
    { 0, 0, 0, ID_of(tune), },         //        "Tune",             // 86
    { 0, 0, 0, 0, },         //        "txtTime0",         // 87
    { 0, 0, 0, 0, },         //        "txtTime1",         // 88
    { 0, 0, 0, 0, },                        //        "txtTime2",         // 89
    { 0, 0, 0, 0, },                        //        "txtTime3",         // 90
    { 1, m1, 0, 0, },                       //        "Units",            // 91
    { 0, 0, 0, ID_of(vel), },               //        "Velocity",         // 92
    { 0, 0, 0, ID_of(climb), },             //        "Vertical Speed",   // 93
    { 3, m1, 0, 0, },                       //        "Video Mode",       // 94
    { 0, 0, 0, ID_of(GPS_sats), },          //        "Visible Sats",     // 95
    { offsetof(Settings,vert_offs),     sizeof(sets.vert_offs),       'b', 0, },         //        "VOS",              // 96
    { 0, 0, 0, ID_of(warn), },              //        "Warnings",         // 97
    { 0, 0, 0, ID_of(windSpeed), },         //        "Wind Speed",       // 98
    { 0, 0, 0, ID_of(WP_dir), },            //        "WP Direction",     // 99
    { 0, 0, 0, ID_of(WP_dist), },           //        "WP Distance",      // 100
    { 0, 0, 0, 0, },                        // #
    { 0, 0, 0, ID_of(Power), },             //        "Power",            // 102
    { 0, 0, 0, ID_of(fDate), },             //        "Date",             // 103
    { 0, 0, 0, ID_of(dayTime), },           //        "Time of day",      // 104
    { 0, 0, 0, ID_of(pMotor), },            //        "Motors",           // 105
    { 0, 0, 0, ID_of(fVibe), },             //        "Vibrations",       // 106
    { 0, 0, 0, ID_of(fVario), },            //        "Variometer",       // 107
    { 0, 0, 0, ID_of(coordLat), },          //        "GPS Coord Lat",    // 108
    { 0, 0, 0, ID_of(coordLon), },          //        "GPS Coord Lon",    // 109

};

static ring_buffer osd_rxrb IN_CCM;
static uint8_t osd_rx_buf[OSD_RX_BUF_SIZE] IN_CCM;

static ring_buffer osd_txrb IN_CCM;
static uint8_t osd_tx_buf[OSD_TX_BUF_SIZE] IN_CCM;

AP_HAL::OwnPtr<F4Light::SPIDevice> osd_spi;
AP_HAL::Semaphore                *osd_spi_sem;

//static volatile byte vas_vsync=false;

mavlink_system_t mavlink_system = {12,1};  // sysid, compid


#ifdef OSD_DMA_TRANSFER
 #define DMA_BUFFER_SIZE 510
    static uint8_t  dma_buffer[DMA_BUFFER_SIZE+1]; // in RAM for DMA
    static uint16_t dma_transfer_length IN_CCM;

static uint8_t shadowbuf[sizeof(OSD::osdbuf)] IN_CCM;

#endif

static bool diff_done;


extern void heartBeat();
extern void writePanels();

void On100ms() {}
void On20ms() {}
void osd_loop();
void vsync_ISR();
void max_do_transfer(const char *buffer, uint16_t len);

static void max7456_cs_off(){
    osd_spi->wait_busy(); // wait for transfer complete
    
    const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
    gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);

    delay_ns100(3);
}

static void max7456_cs_on(){
    const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
    gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
    delay_ns100(1);
}


static uint32_t sem_count=0;

void max7456_on(){
    
    max7456_cs_on();

    osd_spi->set_speed(AP_HAL::Device::SPEED_HIGH);
}

static void max7456_sem_on(){
    
    if(osd_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        max7456_on();
    }
}

void max7456_off(){
    max7456_cs_off();
}

static void max7456_sem_off(){
    max7456_off();
    osd_spi_sem->give(); // give sem on last count
}

void MAX_write(byte addr, byte data){
    max7456_cs_on();
    osd_spi->transfer(addr); // this transfer don't controls CS
    osd_spi->transfer(data);
    max7456_cs_off();
}

byte MAX_read(byte addr){
    max7456_cs_on();
    osd_spi->transfer(addr);      // this transfer don't controls CS
    uint8_t ret = osd_spi->transfer(0xff);
    max7456_cs_off();
    return ret;
}

byte MAX_rw(byte b){
    max7456_cs_on();
    uint8_t ret=osd_spi->transfer(b);
    max7456_cs_off();
    return ret;
}

static uint16_t rdb_ptr IN_CCM;


#ifdef OSD_DMA_TRANSFER
static void prepare_dma_buffer(){
    uint16_t rp;
    uint16_t wp=0;

    uint8_t last_h=0xff;

//    MAX_write(MAX7456_DMM_reg, 0); 
//    MAX_write(MAX7456_VM1_reg, B01000111); 

    memset(dma_buffer,0xff,sizeof(dma_buffer));

    dma_buffer[wp++] = MAX7456_DMM_reg;  dma_buffer[wp++] = 0; 
    dma_buffer[wp++] = MAX7456_VM1_reg;  dma_buffer[wp++] = B01000111; 

    
    // сначала все изменения
    for(rp=0; rp<MAX7456_screen_size ; rp++){
        uint8_t c = OSD::osdbuf[rp];
        if(c != shadowbuf[rp] ){
            if(wp>=DMA_BUFFER_SIZE-6) break;
            uint8_t h = rp>>8;
            if(last_h != h){                
                last_h = h;
                dma_buffer[wp++] = MAX7456_DMAH_reg;  dma_buffer[wp++] = h; 
                if(wp>=DMA_BUFFER_SIZE-6) break;
            }
        
            dma_buffer[wp++] = MAX7456_DMAL_reg;  dma_buffer[wp++] = rp&0xFF; 
            dma_buffer[wp++] = MAX7456_DMDI_reg;  dma_buffer[wp++] = c; 
            shadowbuf[rp] = c;
        }
    }
    
    // а в оставшееся место все остальное по кольцу. таким образом пересылка у нас всегда 500 байт, и на частоте 4.5МГц занимает ~1ms. 
    // длинные пересылки имеют низкий приоритет, и никому не мешают
    while(wp<DMA_BUFFER_SIZE-6){
            uint8_t c = OSD::osdbuf[rdb_ptr];
            uint8_t h = rdb_ptr>>8;
            if(last_h != h){                
                last_h = h;
                dma_buffer[wp++] = MAX7456_DMAH_reg;  dma_buffer[wp++] = h; 
                if(wp>=DMA_BUFFER_SIZE-6) break;
            }
        
            dma_buffer[wp++] = MAX7456_DMAL_reg;  dma_buffer[wp++] = rdb_ptr&0xFF; 
            dma_buffer[wp++] = MAX7456_DMDI_reg;  dma_buffer[wp++] = c; 
            shadowbuf[rdb_ptr] = c;
            rdb_ptr++;
            if(rdb_ptr >= MAX7456_screen_size) rdb_ptr=0; // loop
    }
    
//    dma_buffer[wp++] = MAX7456_VM0_reg;  dma_buffer[wp++] = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode; 
        
    dma_transfer_length = wp;
    diff_done = true;
}
#endif


uint32_t get_word(char *buf, char * &ptr){
    uint32_t sel_len=0;
    uint8_t  sel_id=0;
    
    for(uint32_t i=0; i<ARRAY_SIZE(words); i++){
        uint32_t len=strlen(words[i]);
        if(strncmp(buf, words[i],len)==0){
            if(len > sel_len) { // longest match
                sel_len = len;
                sel_id = i+1;
            }
        }
    }
    ptr=buf+sel_len;
    return sel_id;
}

char * get_lex(char * &ptro){
    char *ptr;
    char *buf = ptro;
    while(*buf && (*buf=='\t' || *buf == ' ')) buf++;
    ptr=buf;
    while(*ptr && *ptr!='\t') ptr++;
    if(*ptr==0) {
        ptro=NULL;
    } else {
        *ptr=0;
        ptro=ptr+1;
    }
    return buf;
    
}

static bool get_flag(char *p) {
    if(!p) return false;
    
    if(*p=='T' || *p=='t' || *p=='1') return true;
    return false;
}

//                          x,         y,           vis,          sign,    Altf,        Alt2,        Alt3,          Alt4,     strings
//                         30         15          False            0       1            1             1              1          A||||
static point create_point(char *px,   char *py, char *pVis,  char *pSign, char *pAlt,  char *pAlt2, char *pAlt3, char *pAlt4,  char *ps ){
    point p;
    p.x = strtoul(px, nullptr, 10);
    p.y = strtoul(py, nullptr, 10);
    p = do_on(p,   get_flag(pVis));
    p = do_sign(p, get_flag(pSign));
    if(get_flag(pAlt))  p=do_alt(p);
    if(get_flag(pAlt2)) p=do_alt2(p);
    if(get_flag(pAlt3)) p=do_alt3(p);
    if(get_flag(pAlt4)) p=do_alt4(p);

//    if(ps) collect_strings(ps); not supported
    return p;
}

#define write_point(n,p) eeprom_write_len((byte *)&p,  OffsetBITpanel * (int)panel_num + n * sizeof(Point),  sizeof(Point) );

static void load_config(){
    File fd = SD.open("eeprom.osd", FILE_READ);
    if (fd) {
        printf("\nLoading OSD config\n");
        char buf[80];
//        memset(buf, 0, sizeof(buf));
        uint32_t panel_num=-1;
        bool is_config=false;
        
        while(fd.gets(buf, sizeof(buf)-1) > 0) {
            // we readed one line
            char *ptr;
            char *p[10];

            uint8_t word=get_word(buf,ptr);
            switch(word) {
            case 0: // not found
            case 101: // #
                continue;
                
            case 47: { // panel
                    char *p2 = get_lex(ptr);
                    panel_num=strtoul(p2, nullptr, 10);
                    uint16_t flags=strtoul(ptr, nullptr, 10);
                    write_point(0,flags);
                }break;
                
            case 19: // config
                is_config=true;
                break;
                
            default:
                char **pp = p;
                memset(p,0,sizeof(p));
                do {
                    *pp++ = get_lex(ptr);
                }while(ptr);
                
                                
                if(is_config){
                    if(pan_tbl[word].size == (uint8_t)-1){ // bit flags
                        uint32_t flags = sets.flags.dw;
                        if(get_flag(p[0])) flags |=  (1<<pan_tbl[word].dst);
                        else               flags &= ~(1<<pan_tbl[word].dst);
                        sets.flags.dw=flags;
                    } else {
                        union {
                            float f;
                            uint8_t b;
                            char buf[8];
                            uint16_t w;
                        } val;
                        
                        switch(pan_tbl[word].fmt){
                        case 'f': // float
                            val.f = atof(p[0]);
                            break;
                        case 'b': // byte
                            val.b=(uint8_t)strtoul(p[0], nullptr, 10);
                            break;
                        case 'c': // char
                            strncpy(val.buf, p[0], 8);
                            break;
                        case 'w':
                            val.w=(uint16_t)strtoul(p[0], nullptr, 10);
                            break;                    
                        default:
                            continue; // ignore this line
                        }                    
                        memmove(         ((uint8_t*)(&sets)) + pan_tbl[word].dst, &val, pan_tbl[word].size);
                        eeprom_write_len(((uint8_t*)(&sets)) + pan_tbl[word].dst, EEPROM_offs(sets) + pan_tbl[word].dst,  pan_tbl[word].size);
                    }
                }else{ // panel
                    uint8_t id = pan_tbl[word].pan_n;
                    if(id) {
                //                           30    15    False   0     1     1     1     1   A||||
                        point po=create_point(p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8] );
                    
                        write_point(id, po); // save to eeprom
                    }
                }
                
                break;
            }
            
        }
        fd.close();

        // we don't save flags when parsing so lets do it now
        eeprom_write_len( &sets.flags.pad[0],  EEPROM_offs(sets) + ((byte *)&sets.flags.pad - (byte *)&sets),  sizeof(sets.flags.pad));

        readSettings(); // re-read new values back, for settings that not in config.osd get all-1 state
        
        sets.CHK1_VERSION = VER;        // set version - EEPROM OK
        sets.CHK2_VERSION = (VER ^ 0x55);
        eeprom_write_len( &sets.CHK1_VERSION,  EEPROM_offs(sets) + ((byte *)&sets.CHK1_VERSION - (byte *)&sets),  2 );
    }
}

static void load_font(){
    const char font[]="font.mcm";
    File fd = SD.open(font, FILE_READ);
    if (fd) {
        char buf[80];
        
        if(fd.gets(buf, sizeof(buf)-1)){
            printf("\nLoading OSD font\n");

            OSD::setPanel(5, 5);
            osd_print_S(PSTR("font uploading "));
            OSD::update();// Show it
            
            char patt[]="MAX7456";
            
            if(strncmp(buf,patt,strlen(patt))==0){
                uint8_t character_bitmap[0x40];
                
                uint16_t font_count = 0;
                byte byte_count = 0;
                byte bit_count=0;

                uint8_t chk=0;
                uint8_t c=0;
                uint8_t last_c;

                uint8_t b=0;

                uint8_t cnt=0;
                while(font_count < 256) {
                    last_c = c;
                    if(fd.read(&c,1)<=0) break; // get a char
                                            
                    switch(c){ // parse and decode mcm file
                    case 0x0A: // line feed - skip
                        if(last_c == 0x0d) continue;
                        // lf without cr cause line end

                    case 0x0d: // carridge return, end of line
                        if (bit_count == 8)  {
                            chk ^= b;
                            character_bitmap[byte_count] = b;
                            b = 0;
                            byte_count++;
                        }
                        bit_count = 0;
                        break;

                    case 0x30: // ascii '0'
                    case 0x31: // ascii '1' 
                        b <<= 1;
                        if(c == 0x31)
                            b += 1;
                        bit_count++;

                        break;

                    default:
                        break;
                    }
            
                    // we have one completed character
                    // write the character to NVM 
                    if(byte_count == 64) {
                        osd.write_NVM(font_count, character_bitmap);
                        byte_count = 0;
                        font_count++;
                        printf(".");
                        chk=0;
                    }                    
                }
                printf("done\n");
            }                    
        }

        fd.close();
//* not in debug mode
        SD.remove(font); // once!
//*/
    }

}

static bool osd_need_redraw = false;
static void * task_handle;

// slowly write all buffer
static void write_buff_to_MAX(bool all){

    max7456_on();
    MAX_write(MAX7456_DMM_reg, 0); 

    // clear internal memory
    uint8_t old_h=0xff;
    for(uint16_t len = MAX7456_screen_size, cnt=0;len--; cnt++){
        uint8_t c= OSD::osdbuf[cnt];
        if(all || c!=0x20) {
            max7456_cs_on();
            uint8_t h = cnt>>8;
            if(old_h!=h){
                MAX_write(MAX7456_DMAH_reg, h);
                old_h = h;
            }
            MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
            MAX_write(MAX7456_DMDI_reg, c);
            max7456_cs_off();
        }
#ifdef OSD_DMA_TRANSFER
        shadowbuf[cnt] = c;
#endif
    }
    max7456_off();
}



void osd_begin(AP_HAL::OwnPtr<F4Light::SPIDevice> spi){

    osd_spi = std::move(spi);
    
    osd_spi_sem = osd_spi->get_semaphore(); // bus semaphore
    {
        const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_CS_PIN];
        gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP);
        gpio_set_speed(pp.gpio_device, pp.gpio_bit, GPIO_speed_100MHz); 
        gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
    }

#ifdef BOARD_OSD_RESET_PIN
    {
        const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_RESET_PIN];
        gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP);
        gpio_set_speed(pp.gpio_device, pp.gpio_bit, GPIO_speed_25MHz); 
        gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
        delayMicroseconds(50);
        gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
        delayMicroseconds(120);
    }
#endif


    rb_init(&osd_rxrb, OSD_RX_BUF_SIZE, osd_rx_buf);
    rb_init(&osd_txrb, OSD_TX_BUF_SIZE, osd_tx_buf);

    OSD_EEPROM::init();

    // clear memory
    memset(OSD::osdbuf,0x20, sizeof(OSD::osdbuf));
#ifdef OSD_DMA_TRANSFER
    memset(shadowbuf,  0x20, sizeof(shadowbuf));
#endif

/*
    lets try to load settings from SD card
*/
    load_config();

    readSettings();

    doScreenSwitch(); // set vars for startup screen

    if( sets.CHK1_VERSION != VER || sets.CHK2_VERSION != (VER ^ 0x55)) { // wrong version
        lflags.bad_config=1;
        
        // some useful defaults
        sets.OSD_BRIGHTNESS = 2;
        sets.horiz_offs = 0x20;
        sets.vert_offs  = 0x10;
        
    }

    while(millis()<1000) { // delay initialization until video stabilizes
        hal_yield(1000);
    }

    max7456_sem_on();

    write_buff_to_MAX(true);


    for(uint8_t i=0; i<100; i++) {
        osd.init();    // Start display
        
        max7456_on();
        uint8_t vm0 = MAX_read(MAX7456_VM0_reg | MAX7456_reg_read); // check register
        max7456_off();

        uint8_t patt = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;

        if(vm0==patt) break;
    }

    max7456_off();

    load_font();

    max7456_sem_off();

#define REL_1 int(RELEASE_NUM/100)
#define REL_2 int((RELEASE_NUM - REL_1*100 )/10) 
#define REL_3 int((RELEASE_NUM - REL_1*100 - REL_2*10  )) 

    if(sets.FW_VERSION[0]!=(REL_1 + '0') || sets.FW_VERSION[1]!=(REL_2 + '0') || sets.FW_VERSION[2]!=(REL_3 + '0') ){
        sets.FW_VERSION[0]=REL_1 + '0';
        sets.FW_VERSION[1]=REL_2 + '0';
        sets.FW_VERSION[2]=REL_3 + '0';

        eeprom_write_len( sets.FW_VERSION,  EEPROM_offs(sets) + ((uint8_t *)sets.FW_VERSION - (uint8_t *)&sets),  sizeof(sets.FW_VERSION) );
    }
    
    logo();

    
#ifdef BOARD_OSD_VSYNC_PIN
    Revo_hal_handler h = { .vp = vsync_ISR };
    
    GPIO::_attach_interrupt(BOARD_OSD_VSYNC_PIN, h.h, RISING, VSI_INT_PRIORITY);
#endif

    task_handle = Scheduler::start_task(OSDns::osd_loop, SMALL_TASK_STACK); // 
    Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // less than main task
    Scheduler::set_task_period(task_handle, 10000);              // 100Hz 
}

// all task is in one thread so no sync required

void osd_loop() {
    if(osd_need_redraw){ // если была отложенная передача
        osd_need_redraw=false;
        
        OSD::update();           
        Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low
    }

    uint32_t pt=millis();

    seconds = pt / 1000;

    osd_dequeue(); // we MUST parse input even in case  of bad config because it is the only way to communicate

    if(pt < BOOTTIME || lflags.bad_config){ // startup delay for fonts or EEPROM error
            logo();
            return;
    }

#if defined(MAV_REQUEST) && (defined(USE_MAVLINK) || defined(USE_MAVLINKPX4))
    if(apm_mav_system && !lflags.mav_request_done){ // we got HEARTBEAT packet and still don't send requests
        for(uint8_t n = 3; n >0; n--){
            request_mavlink_rates();        //Three times to certify it will be readed
            delay_150();
        }
        lflags.mav_request_done=1;
    }
#endif

    if(lflags.got_data){ // были свежие данные - обработать

        pan_toggle(); // проверить переключение экранов

        if(!lflags.need_redraw) {
            lflags.need_redraw=1;
            vsync_wait=1; // будем ждать прерывания
        }

        lflags.got_data=0; // данные обработаны
    }
    
    if( lflags.need_redraw) {                 
        lflags.need_redraw=0; // экран перерисован

        setHomeVars();   // calculate and set Distance from home and Direction to home

        setFdataVars();  // накопление статистики и рекордов

        writePanels();   // writing enabled panels (check OSD_Panels Tab)

#ifdef OSD_DMA_TRANSFER
        prepare_dma_buffer(); // prepare diff with addresses
#endif

        update_screen = 1; // пришли данные, надо перерисовать экран
    }

    if(pt > timer_20ms){
        timer_20ms+=20;
        On20ms();
        
        if(update_screen && vsync_wait && (millis() - vsync_time)>50){ // прерывания остановились - с последнего прошло более 50мс
            vsync_wait=0; // хватит ждать
            Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // equal to main 
            OSD::update(); // обновим принудительно (и далее будем обновлять каждые 20мс)
            update_screen = 0;
            Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY);
        }
    }

    if(pt > timer_100ms){
        timer_100ms+= 100;
        On100ms(); 

        lflags.flag_01s = !lflags.flag_01s;

        if(lflags.flag_01s) {

            if(skip_inc) {
                skip_inc++;

                if(skip_inc >=3){
                    count02s++;
                    skip_inc=0; // we go again
                }

            } else
                count02s++;
        }
//      count01s++;
    }

    if(pt > timer_500ms){
        timer_500ms+= 500;
        lflags.got_data=1; // каждые полсекунды принудительно
        update_screen = 1; 

        lflags.flag_05s = 1;

        count05s++;

        lflags.blinker = !lflags.blinker;
        if(lflags.blinker) {
    //        seconds++;
            lflags.one_sec_timer_switch = 1; // for warnings

            if(lflags.got_date) day_seconds++; // if we has GPS time - let it ticks

            if( vas_vsync && vsync_count < 5) { // при частоте кадров их должно быть 25 или 50
                                                    // но есть платы где эта нога не подключена. Китай...
                max7456_err_count++;
                if(max7456_err_count>3) { // 3 seconds bad sync
#ifdef DEBUG   
                    printf(PSTR("restart MAX! vsync_count=%d\n"),vsync_count);
#endif
                    osd.reset();    // restart MAX7456
                }
            } else  max7456_err_count=0;

            vsync_count=0;

            heartBeat();
        }
    }
}




void vsync_ISR(){
    vas_vsync=true;
    vsync_wait=0;       // отметить его наличие

    vsync_count++; // считаем кадровые прерывания
    vsync_time=millis(); // и отметим его время

    if(update_screen) { // there is data for screen
        osd_need_redraw=true;
        Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // higher than all drivers so it will be scheduled just after semaphore release
        Scheduler::task_resume(task_handle); // task should be finished at this time so resume it
        update_screen = 0;
    }
}



// is there any chars in ring buffer?
int16_t osd_available(){
    return rb_full_count(&osd_rxrb);
}

void osd_queue(uint8_t c) {    // push bytes from OSD to FC around in the ring buffer
    uint8_t cnt=10;
    while(rb_is_full(&osd_rxrb)) {
        hal_yield(0);
        if(--cnt == 0) return; // destination don't listen
    }
    rb_push_insert(&osd_rxrb, c);
}


int16_t osd_getc(){ // get char from ring buffer
    return rb_remove(&osd_rxrb);
}

uint32_t osd_txspace() {
    return osd_txrb.size - rb_full_count(&osd_txrb);
}


void osd_putc(uint8_t c){ 
    uint8_t cnt=10;
    while(rb_is_full(&osd_txrb)) {
        Scheduler::set_task_priority(task_handle, OSD_HIGH_PRIORITY); // to run in time of yield()
        hal_yield(0);
        if(--cnt == 0) break; // destination don't listen
    }
    rb_push_insert(&osd_txrb, c);
    Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low
}

void osd_dequeue() {
    Scheduler::set_task_priority(task_handle, 100); // equal to main to not overflow buffers on packet decode

    while(!rb_is_empty(&osd_txrb)) {
        extern bool mavlink_one_byte(char c);
        char c = rb_remove(&osd_txrb);
    
        if(mavlink_one_byte(c)) lflags.got_data=true;
    }
    Scheduler::set_task_priority(task_handle, OSD_LOW_PRIORITY); // restore priority to low

}


static uint8_t max_err_cnt=0;

void update_max_buffer(const uint8_t *buffer, uint16_t len){
    max7456_sem_on();

    uint16_t cnt=0;
    
    
#if defined(OSD_DMA_TRANSFER) && 0
//    MAX_write(MAX7456_DMM_reg, 0); 
//    MAX_write(MAX7456_VM1_reg, B01000111);  - in dma_buffer

    if(!diff_done){
        prepare_dma_buffer(); 
    }

    if(dma_transfer_length) {
        osd_spi->transfer(dma_buffer, dma_transfer_length, NULL, 0);    // diff already prepared

        dma_transfer_length = 0;
    }
    diff_done = false;


    max7456_cs_off();
    uint8_t patt = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;
    max7456_cs_on();
    uint8_t vm0 = MAX_read(MAX7456_VM0_reg | MAX7456_reg_read);
    max7456_cs_off();
        
    if(vm0 != patt) {
        max_err_cnt++;
        if(max_err_cnt<3) {
            OSD::hw_init(); // first try without reset
        } else {
                // 3 errors together - nothing helps :(
#ifdef BOARD_OSD_RESET_PIN
            {
                const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_RESET_PIN];
                gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
                delayMicroseconds(50);
                gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
                delayMicroseconds(120);
            }
#endif
            OSD::init();
            max_err_cnt=0; 
            write_buff_to_MAX(false); // restore screen
        }
//            MAX_write(MAX7456_VM0_reg, patt);         
    } else {
        max_err_cnt=0;
    }

#elif 1
    uint8_t patt = MAX7456_ENABLE_display | MAX7456_SYNC_autosync | OSD::video_mode;
    max7456_cs_on();
    uint8_t vm0 = MAX_read(MAX7456_VM0_reg | MAX7456_reg_read);
        
    if(vm0 != patt) {
        max_err_cnt++;
        if(max_err_cnt<3) {
            OSD::hw_init(); // first try without reset
        } else {
                // 3 errors together - nothing helps :(
#ifdef BOARD_OSD_RESET_PIN
            {
                const stm32_pin_info &pp = PIN_MAP[BOARD_OSD_RESET_PIN];
                gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW);
                delayMicroseconds(50);
                gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH);
                delayMicroseconds(120);
            }
#endif
            OSD::init();
            max_err_cnt=0; 
        }
    } else {
        max_err_cnt=0;
    }

    MAX_write(MAX7456_DMAH_reg, 0);
    MAX_write(MAX7456_DMAL_reg, 0);
    MAX_write(MAX7456_DMM_reg, 1); // автоинкремент адреса

    max7456_cs_off();
    
    if(osd_spi->send_strobe(buffer, len)!=len) {
    /*/// for debug - mark last written char
        MAX_rw(0x86); // finish transfer
    //*///
        MAX_rw(0xff); // finish transfer
    }
    
#elif 0
    MAX_write(MAX7456_DMAH_reg, 0);
    MAX_write(MAX7456_DMAL_reg, 0);
    MAX_write(MAX7456_DMM_reg, 0); 

// try to send just diffenence - don't clears last chars
    uint8_t last_h=0;
    while(len--){
        if(*buffer != shadowbuf[cnt]){
            uint8_t h = cnt>>8 ;
            if(last_h != h){
                MAX_write(MAX7456_DMAH_reg, h);
                last_h = h;
            }
            MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
            MAX_write(MAX7456_DMDI_reg, *buffer);
            shadowbuf[cnt] = *buffer;
        }
        buffer++;
        cnt++;
    }

#elif 0

// a try to do writes in software strobe mode
    MAX_write(MAX7456_DMAH_reg, 0);
    MAX_write(MAX7456_DMAL_reg, 0);
    MAX_write(MAX7456_DMM_reg, 1); // автоинкремент адреса
    max7456_cs_off();
    while(len--){
        max7456_cs_on();
        // osd_spi->transfer(*buffer++); MAX7456
        MAX_rw(*buffer++);
        buffer++;
        cnt++;
        osd_spi->wait_busy();
        max7456_cs_off();
    }
    max7456_cs_on();
    MAX_write(MAX7456_DMM_reg, 0); // автоинкремент адреса
#else
// just write all to MAX
    MAX_write(MAX7456_DMAH_reg, 0);
    MAX_write(MAX7456_DMAL_reg, 0);
    MAX_write(MAX7456_DMM_reg, 0); 

    uint8_t last_h=0;
    while(len--){
        uint8_t h = cnt>>8 ;
        if(last_h != h){
            MAX_write(MAX7456_DMAH_reg, h);
            last_h = h;
        }
        
        MAX_write(MAX7456_DMAL_reg, cnt&0xFF);
        MAX_write(MAX7456_DMDI_reg, *buffer);
        buffer++;
        cnt++;
    }

#endif    

    max7456_sem_off();
}



} // namespace


#endif