main/co2meter.c

/**
 * @file co2meter.c
 * @brief co2meter project.
 */

#include "config.h"

static const char *TAG = "co2meter";

#define PIN_SDA 		(CONFIG_I2C_MASTER_SDA)
#define PIN_SCL			(CONFIG_I2C_MASTER_SCL)
#define ROT_ENC_A_GPIO		(CONFIG_ROT_ENC_A_GPIO)
#define ROT_ENC_B_GPIO		(CONFIG_ROT_ENC_B_GPIO)
#define ROT_ENC_SW_GPIO		(CONFIG_ROT_ENC_SW_GPIO)
#define INACTIVITY		480					///< Time in 250 mSec units.

#define	EDIT_TYPE_TEXT		0					///< Editor type is text
#define	EDIT_TYPE_INT		1					///< Editor type is integer
#define	EDIT_TYPE_FLOAT		2					///< Editor type is float


int					Main_Loop1 = ML1_INIT;		///< Loop 1 init
int					Main_Loop2 = -1;		///< Loop 2 invalid
int					New_Loop2 = ML2_DONE;		///< Loop 2 new state
bool                            	System_TimeOk = false;          ///< System time status
time_t                          	now;                            ///< Current time
struct tm                       	timeinfo;                       ///< Current time structure
char                            	strftime_buf[64];               ///< Time buffer
static RTC_DATA_ATTR struct timeval	sleep_enter_time;
static TaskHandle_t			xTaskDS18B20 = NULL;
static TaskHandle_t			xTaskADC = NULL;
static TaskHandle_t			xTaskWifi = NULL;
static TaskHandle_t			xTaskMQTT = NULL;
const esp_app_desc_t			*app_desc = NULL;		///< Application description
u8g2_t					u8g2;				///< A structure which will contain all the data for one display
rotary_encoder_info_t			rinfo = { 0 };			///< Rotary encoder record
rotary_encoder_event_t			event = { 0 };
QueueHandle_t				event_queue;
static int				PushDuration = 0;		///< Duration of the pushed button

extern unit_t				units[3];			///< Pressure test units
extern SemaphoreHandle_t		xSemaphoreUnits;		///< Units lock semaphore
extern DS18B20_State            	*ds18b20_state;         	///< DS18B20 state
extern SemaphoreHandle_t        	xSemaphoreDS18B20;      	///< DS18B20 lock semaphore
extern ADC_State                	*adc_state;             	///< ADC state
extern SemaphoreHandle_t        	xSemaphoreADC;          	///< ADC lock semaphore
extern WIFI_State			*wifi_state;			///< WiFi state
extern int				count_pub;			///< Published MQTT messages in transit
static xQueueHandle			gpio_evt_queue = NULL;		///< Rotary pushbutton queue
static int				usertimer = 0;			///< User inactive timeout


/**
 * @brief Get a keyboard character from the rotary encoder.
 * @param curkey The referenced value if the key being edited. NOTE, start at 0 for a new char??
 * @param type The edittype, all values, integer or float.
 * @param x The x position on the screen.
 * @param y The y position on the screen.
 * @return 1 if short keypress, meaning enter key. 2 if long press, enter key and editing is ready.
 */
int getkey(int *curkey, int type, int x, int y)
{
    int	key = *curkey;
    int	rc = 0;

    u8g2_DrawHLine(&u8g2, x, y+3, 12);
    u8g2_SendBuffer(&u8g2);

    for (;;) {
    	if (xQueueReceive(event_queue, &event, 100 / portTICK_PERIOD_MS) == pdTRUE) {
	    usertimer = INACTIVITY;
	    if (event.state.position != 0) {

		u8g2_SetDrawColor(&u8g2, 0);
		u8g2_DrawGlyph(&u8g2, x, y, key);
		u8g2_SetDrawColor(&u8g2, 1);
		u8g2_SendBuffer(&u8g2);

	    	if (event.state.position > 0) {
		    if (key == 126)
		    	key = 171;
		    else if (key < 126)
		    	key++;
	    	} else if (event.state.position < 0) {
		    if (key == 171)
		    	key = 126;
		    else if (key > 32)
		    	key--;
		}

		ESP_ERROR_CHECK(rotary_encoder_reset(&rinfo));
	    	u8g2_DrawGlyph(&u8g2, x, y, key);
	    	u8g2_SendBuffer(&u8g2);
	    }
    	} else {
	    if (PushDuration) {
		if (PushDuration > 500)
		    rc = 2;
		else
		    rc = 1;
	    	PushDuration = 0;
		break;
	    }
    	}
    }
    u8g2_SetDrawColor(&u8g2, 0);
    u8g2_DrawHLine(&u8g2, x, y+3, 12);
    u8g2_SetDrawColor(&u8g2, 1);
    u8g2_SendBuffer(&u8g2);

    *curkey = key;
    return rc;
}


/**
 * @brief Editor using the rotary switch.
 * @param label The label of the edit field.
 * @param txt The string to edit.
 * @param errmsg The error message if needed.
 * @param len The maximum length for the string.
 * @param type The edit type.
 */
void rotary_editer(char *label, char *txt, char *errmsg, int len, int type)
{
    char        buf[65];
    int		key, x, y, rc;

    u8g2_ClearBuffer(&u8g2);
    u8g2_DrawHLine(&u8g2, 0, 14, 128);
    u8g2_DrawHLine(&u8g2, 0, 49, 128);
    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tf);
    sprintf(buf, "Edit %s", label);
    u8g2_DrawStr(&u8g2,0,12,buf);

    if (strlen(errmsg)) {
	u8g2_SetFont(&u8g2, u8g2_font_t0_12b_tf);
	u8g2_DrawStr(&u8g2, 0, 61, errmsg);
    }
    u8g2_SetFont(&u8g2, u8g2_font_t0_12_tf);
    y = 36;
    u8g2_DrawStr(&u8g2, 0, y, txt);
    u8g2_SendBuffer(&u8g2);

    for (;;) {
	x = u8g2_GetUTF8Width(&u8g2, txt);
	key = 'a';
	rc = getkey(&key, type, x, y);
	if (rc == 1) {
	    if (key >= 32 && key <= 126 && strlen(txt) < len) {
		txt[strlen(txt) + 1] = '\0';
                txt[strlen(txt)] = key;
	    } else if (key == 171 && strlen(txt)) {
		// delete key
		txt[strlen(txt) - 1] = '\0';
	    }
printf("strlen %d  x %d  key %d\n", strlen(txt), x, key);
	} else if (rc == 2) {
	    break;
	}
    }
}


/**
 * @brief Write a menu line on the display.
 * @param bright Display the line with a bold or normal font.
 * @param x The horizontal start position of the line.
 * @param y The vertical bottom of the line position.
 * @param format The formatted data to display.
 */
void menu_line(int bright, int x, int y, const char *format, ...)
{
    char        buf[65];
    va_list     va_ptr;

    if (bright)
        u8g2_SetFont(&u8g2, u8g2_font_t0_12b_tr);
    else
        u8g2_SetFont(&u8g2, u8g2_font_t0_12_tr);

    va_start(va_ptr, format);
    vsnprintf(buf, 65, format, va_ptr);
    va_end(va_ptr);

    u8g2_DrawStr(&u8g2, x, y, buf);
}


/**
 * @brief Clear the display and prepare the top of the display.
 * @format The formatted data to display at the top.
 */
void screen_top(const char *format, ...)
{
    char        buf[65];
    va_list     va_ptr;

    va_start(va_ptr, format);
    vsnprintf(buf, 65, format, va_ptr);
    va_end(va_ptr);

    u8g2_ClearBuffer(&u8g2);
    u8g2_DrawHLine(&u8g2, 0, 14, 128);

    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
    u8g2_uint_t w = u8g2_GetStrWidth(&u8g2, buf);
    u8g2_DrawStr(&u8g2, (128 - w) / 2,12, buf);
}


/**
 * @brief The splash screen shown during cold boot or user wakeup.
 */
void screen_splash()
{
    screen_top("CO2 meter %s", app_desc->version);

    u8g2_SetFont(&u8g2, u8g2_font_t0_22b_tf);
    u8g2_uint_t w = u8g2_GetUTF8Width(&u8g2, "START");
    u8g2_DrawUTF8(&u8g2, (128 - w) / 2,50, "START");

    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0); // wake up display
}


/**
 * @brief The main overview screen.
 */
void screen_main()
{
    char	buf[65];
    int		i;

    screen_top("CO2 meter %s", app_desc->version);

    if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE) {

    	u8g2_SetFont(&u8g2, u8g2_font_t0_22b_tf);
    	sprintf(buf, "%.1f °C", units[0].temperature / 1000.0);
    	u8g2_uint_t w = u8g2_GetUTF8Width(&u8g2, buf);
    	u8g2_DrawUTF8(&u8g2, (128 - w) / 2,40, buf);
    	u8g2_SetFont(&u8g2, u8g2_font_t0_18b_tf);

	for (i = 0; i < 3; i++) {
    	    sprintf(buf, "%.1f", units[i].pressure / 1000.0);
    	    w = u8g2_GetUTF8Width(&u8g2, buf);
    	    u8g2_DrawUTF8(&u8g2, ((42 - w) / 2) + i * 43,63, buf);
	}
	xSemaphoreGive(xSemaphoreUnits);
    }
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0); // wake up display
}


/**
 * @brief The unit display screen.
 * @param no The unit index number.
 */
void screen_unit(int no)
{
    char        buf[65];

    if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE) {

	screen_top("Unit %d %s", no + 1, units[no].mode ? "On":"Off");

    	u8g2_SetFont(&u8g2, u8g2_font_t0_22b_tf);
    	sprintf(buf, "%.1f °C", units[no].temperature / 1000.0);
    	u8g2_uint_t w = u8g2_GetUTF8Width(&u8g2, buf);
    	u8g2_DrawUTF8(&u8g2, (128 - w) / 2,40, buf);

    	sprintf(buf, "%.2f bar", units[no].pressure / 1000.0);
    	w = u8g2_GetUTF8Width(&u8g2, buf);
    	u8g2_DrawUTF8(&u8g2, (128 - w) / 2,63, buf);

	xSemaphoreGive(xSemaphoreUnits);
    }
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0); // wake up display
}


/**
 * @brief The unit zero setup screen.
 * @param no The unit index number.
 * @param sub The submenu index number.
 */
void screen_unit_zero(int no, int sub)
{
    screen_top("Unit %d zero mV", no + 1);
    menu_line(       0, 2, 25, "Current   %d", units[no].pressure_zero);
    menu_line(sub == 0, 2, 37, "New value %d", units[no].pressure_voltage / (adc_state->Batt_voltage / 1000));
    menu_line(sub == 1, 2, 49, "Return");
printf("current %d  p_voltage %d  batt %d\n", units[no].pressure_zero, units[no].pressure_voltage, adc_state->Batt_voltage);
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


/**
 * @brief The unit setup screen.
 * @param no The unit index number.
 * @param sub The submenu index number.
 */
void screen_unit_setup(int no, int sub)
{
    screen_top("Unit %d setup", no + 1);
    menu_line(sub == 0, 2, 25, "Mode    %s", units[no].mode ? "ON":"OFF");
    menu_line(sub == 1, 2, 37, "Zero mV %d", units[no].pressure_zero);
    menu_line(sub == 2, 2, 49, "DS18B20 %s", units[no].temperature_rom_code);
    menu_line(sub == 3, 2, 61, "Return");
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


void screen_wifi()
{
    char	buf[65];

    screen_top("WiFi Status");
    snprintf(buf, 65, "SSID %s", wifi_state->STA_ssid);
    u8g2_DrawStr(&u8g2, 1, 28, buf);
    snprintf(buf, 65, "Online %s", wifi_state->STA_online ? "Yes":"No");
    u8g2_DrawStr(&u8g2, 1, 43, buf);
    snprintf(buf, 65, "RSSI %d", wifi_state->STA_rssi);
    u8g2_DrawStr(&u8g2, 1, 59, buf);
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


void screen_wifi_setup(int sub)
{
    screen_top("WiFi Setup");
    menu_line(sub == 0, 2, 25, "Connect");
    menu_line(sub == 1, 2, 37, "New");
    menu_line(sub == 2, 2, 49, "Delete");
    menu_line(sub == 3, 2, 61, "Return");
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


void screen_network()
{
    screen_top("Network Status");
    menu_line(0, 1, 25, "IP   %s", wifi_state->STA_ip);
    menu_line(0, 1, 37, "Mask %s", wifi_state->STA_nm);
    menu_line(0, 1, 49, "GW   %s", wifi_state->STA_gw);
    menu_line(0, 1, 61, "Name %s", config.hostname);
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


void screen_mqtt()
{
    screen_top("MQTT Status");
    menu_line(0, 1, 25, "serv %s", config.mqtt_server);
    menu_line(0, 1, 37, "port %d", config.mqtt_port);
    menu_line(0, 1, 49, "user %s", config.mqtt_user);
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


void screen_update()
{
    screen_top("Update firmware");
    menu_line(0, 1, 43, "Push to update");
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


/**
 * @brief Fatal messages on the screen.
 * @param e1 The first line.
 * @param e2 The second line.
 */
void screen_fatal(char *e1, char *e2)
{
    u8g2_SetFont(&u8g2, u8g2_font_t0_15_tr);
    u8g2_DrawStr(&u8g2,2,12,e1);
    u8g2_DrawStr(&u8g2,2,24,e2);
    u8g2_SendBuffer(&u8g2);
    u8g2_SetPowerSave(&u8g2, 0);
}


/**
 * @brief Interrupt service routine for the rotary pushbutton.
 */
static void IRAM_ATTR gpio_isr_handler(void* arg)
{
    uint32_t gpio_num = (uint32_t) arg;
    xQueueSendFromISR(gpio_evt_queue, &gpio_num, NULL);
}


/**
 * @brief GPIO queue task. See if there is a rotary pushbutton event on the queue.
 */
static void gpio_task(void* arg)
{
    uint32_t		io_num;
    static int64_t	pushed = 0;

    for(;;) {
        if (xQueueReceive(gpio_evt_queue, &io_num, portMAX_DELAY)) {
	    if (io_num == ROT_ENC_SW_GPIO) {
		if (gpio_get_level(io_num) == 0) {
		    pushed = esp_timer_get_time();
		    PushDuration = 0;
		} else if (gpio_get_level(io_num) == 1) {
		    PushDuration = (esp_timer_get_time() - pushed) / 1000;
		    ESP_LOGI(TAG, "GPIO rotary button intr, val: %d time: %d", gpio_get_level(io_num), PushDuration);
		}
	    } else {
            	ESP_LOGE(TAG, "GPIO[%d] unknown intr, val: %d", io_num, gpio_get_level(io_num));
	    }
	    usertimer = INACTIVITY;
        }
    }
}


/**
 * @brief Select new menu number on a postitive or negative rotary position.
 * @param pos The new position, positive, negative or zero.
 * @param next_menu The selected menu if rotated clockwise.
 * @param prev_menu The selected menu if rotated counter-clockwise.
 */
static void rotate_to_menu(rotary_encoder_position_t pos, int next_menu, int prev_menu)
{
    if (pos > 0)
	New_Loop2 = next_menu;
    else if (pos < 0)
	New_Loop2 = prev_menu;
    ESP_ERROR_CHECK(rotary_encoder_reset(&rinfo));
}


/**
 * @brief Rotate subscreens numbers.
 * @param pos The new position, positive, negative or zero.
 * @param min The lowest number. If already at the lowest, select the highest.
 * @param max The highest number. If already at the highest, select the lowest.
 * @param cursub The subscreen number by reference. This is updated with the new number.
 * @return Returns true if a new number is selected, false if nothing changed.
 */
bool rotate_to_sub(rotary_encoder_position_t pos, int min, int max, int *cursub)
{
   int	sub = *cursub;
   bool	rc = false;

   if (pos > 0) {
	if (sub < max)
	    sub++;
	else
	    sub = min;
	ESP_ERROR_CHECK(rotary_encoder_reset(&rinfo));
	rc = true;
    } else if (pos < 0) {
	if (sub > min)
	    sub--;
	else
	    sub = max;
	ESP_ERROR_CHECK(rotary_encoder_reset(&rinfo));
	rc = true;
    }

    *cursub = sub;
    return rc;
}


void app_main()
{
    struct timeval	now;
    gettimeofday(&now, NULL);
    int			sleep_time_ms = (now.tv_sec - sleep_enter_time.tv_sec) * 1000 + (now.tv_usec - sleep_enter_time.tv_usec) / 1000;
    esp_err_t           ret;
    char		txt[65];

    Main_Loop1 = ML1_INIT;
    Main_Loop2 = -1;

    /*
     * Setup the OLED display.
     * See: https://github.com/nkolban/esp32-snippets/blob/master/hardware/displays/U8G2/
     */
    u8g2_esp32_hal_t u8g2_esp32_hal = U8G2_ESP32_HAL_DEFAULT;
    u8g2_esp32_hal.sda = PIN_SDA;
    u8g2_esp32_hal.scl = PIN_SCL;
    u8g2_esp32_hal_init(u8g2_esp32_hal);

    u8g2_Setup_sh1106_i2c_128x64_noname_f(&u8g2, U8G2_R0, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb);  // init u8g2 structure
    u8x8_SetI2CAddress(&u8g2.u8x8, 0x78);
    u8g2_InitDisplay(&u8g2); // send init sequence to the display, display is in sleep mode after this,

    app_desc = esp_ota_get_app_description();

    switch (esp_sleep_get_wakeup_cause()) {
        case ESP_SLEEP_WAKEUP_EXT1: {
	    ESP_LOGI(TAG, "Starting from deep sleep, Rotary switch pressed");
	    New_Loop2 = ML2_INIT;
	    screen_splash();
            break;
        }
        case ESP_SLEEP_WAKEUP_TIMER: {
	    ESP_LOGI(TAG, "Starting from deep sleep, timer wakeup after %dms", sleep_time_ms);
            break;
        }
        case ESP_SLEEP_WAKEUP_UNDEFINED:
        default:
            ESP_LOGI(TAG, "Starting from hard reset");
	    screen_splash();
    }

    const int wakeup_time_sec = 55;
    ESP_LOGI(TAG, "Enabling timer wakeup, %ds", wakeup_time_sec);
    esp_sleep_enable_timer_wakeup(wakeup_time_sec * 1000000);
    const uint64_t ext_wakeup_pin_1_mask = 1ULL << ROT_ENC_SW_GPIO;

    ESP_LOGI(TAG, "Enabling EXT1 wakeup on pin GPIO%d", ROT_ENC_SW_GPIO);
    esp_sleep_enable_ext1_wakeup(ext_wakeup_pin_1_mask, ESP_EXT1_WAKEUP_ALL_LOW);

    // Isolate GPIO12 pin from external circuits. This is needed for modules
    // which have an external pull-up resistor on GPIO12 (such as ESP32-WROVER)
    // to minimize current consumption.
//    rtc_gpio_isolate(GPIO_NUM_12);

    /*
     * Initialize NVS
     */
    ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    /*
     * Setup SPIFFS filesystem
     */
    ESP_LOGI(TAG, "Initializing SPIFFS");
    esp_vfs_spiffs_conf_t conf = {
        .base_path = "/spiffs",
        .partition_label = NULL,
        .max_files = 5,
        .format_if_mount_failed = true
    };

    /*
     * Use settings defined above to initialize and mount SPIFFS filesystem.
     * Note: esp_vfs_spiffs_register is an all-in-one convenience function.
     */
    ret = esp_vfs_spiffs_register(&conf);

    if (ret != ESP_OK) {
        if (ret == ESP_FAIL) {
            ESP_LOGE(TAG, "Failed to mount or format filesystem");
        } else if (ret == ESP_ERR_NOT_FOUND) {
            ESP_LOGE(TAG, "Failed to find SPIFFS partition");
        } else {
            ESP_LOGE(TAG, "Failed to initialize SPIFFS (%d)", ret);
        }
	screen_fatal("SPIFFS:", "init error");
        return; // Stop application.
    }

    size_t total = 0, used = 0;
    ret = esp_spiffs_info(NULL, &total, &used);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to get SPIFFS partition information");
	screen_fatal("SPIFFS:", "partition error");
        return; // Stop application.
    } else {
        ESP_LOGI(TAG, "Partition size: %d, used: %d - %d%%", total, used, (used * 100) / total);
    }

    // Just to debug, list the /spiffs filesystem.
#if 0
    DIR *dir = opendir("/spiffs");
    struct dirent* de = readdir(dir);
    while (de) {
        if (de->d_type == DT_REG) {
            printf("F ");
        }
        if (de->d_type == DT_DIR) {
            printf("D ");
        }
        printf("%s\n", de->d_name);
        de = readdir(dir);
    }
    closedir(dir);
#endif

    /*
     * Read or create configuration
     */
    read_config();
    read_units();

//add_station((uint8_t *)"MBSE_WLR", (uint8_t *)"abcjkltuv");
//remove_station((uint8_t *)"MBSE_WLP");

    /*
     * Create FreeRTOS tasks
     */
    xSemaphoreDS18B20 = xSemaphoreCreateMutex();
    xSemaphoreADC = xSemaphoreCreateMutex();
    xSemaphoreUnits = xSemaphoreCreateMutex();

    xTaskCreate(&task_ds18b20, "task_ds18b20",  2560, NULL, 8, &xTaskDS18B20);
    xTaskCreate(&task_adc,     "task_adc",      2560, NULL, 8, &xTaskADC);
    esp_log_level_set("wifi", ESP_LOG_ERROR);
    xTaskCreate(&task_wifi,    "task_wifi",     4096, NULL, 3, &xTaskWifi);
    vTaskDelay(10 / portTICK_PERIOD_MS);
    esp_log_level_set("MQTT_CLIENT", ESP_LOG_ERROR);
    xTaskCreate(&task_mqtt,    "task_mqtt",     4096, NULL, 5, &xTaskMQTT);

    /*
     * Setup the Rotary Encoder.
     * esp32-rotary-encoder requires that the GPIO ISR service is 
     * installed before calling rotary_encoder_register()
     */
    ESP_ERROR_CHECK(gpio_install_isr_service(0));
    ESP_ERROR_CHECK(rotary_encoder_init(&rinfo, ROT_ENC_A_GPIO, ROT_ENC_B_GPIO));
    ESP_ERROR_CHECK(rotary_encoder_enable_half_steps(&rinfo, false));

    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_PIN_INTR_ANYEDGE;
    io_conf.pin_bit_mask = (1ULL << ROT_ENC_SW_GPIO);
    io_conf.mode = GPIO_MODE_INPUT;
    gpio_config(&io_conf);

    gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));
    xTaskCreate(gpio_task, "gpio_task", 2048, NULL, 10, NULL);

    gpio_isr_handler_add(ROT_ENC_SW_GPIO, gpio_isr_handler, (void*) ROT_ENC_SW_GPIO);

    // Create a queue for events from the rotary encoder driver.
    // Tasks can read from this queue to receive up to date position information.
    event_queue = rotary_encoder_create_queue();
    ESP_ERROR_CHECK(rotary_encoder_set_queue(&rinfo, event_queue));

    /*
     * Main application loop.
     */
    while (1) {

	ESP_LOGI(TAG, "Entered app loop");
	//event = { 0 };
	int sub = 0;
	u8g2_SetPowerSave(&u8g2, 1);

	/* Measure process or user input via rotary switch */
	while (1) {
	    switch (Main_Loop1) {
		case ML1_INIT:
		    ESP_LOGI(TAG, "Loop timer: Init");
		    // If configured do ML1_CONNECT
		    Main_Loop1 = ML1_CONNECT;
		    requestWiFi_system(true);
		    request_ds18b20();
		    request_adc();
		    break;

		case ML1_CONNECT:
                    if (ready_WiFi()) {
                        Main_Loop1 = ML1_MQTT_CONNECT;
			if (Main_Loop2 == ML2_WIFI)
			    screen_wifi();
		    }
                    break;

		case ML1_MQTT_CONNECT:
		    if (ready_ds18b20() && ready_adc()) {
			connect_mqtt(true);
			Main_Loop1 = ML1_WAITCON;
			ESP_LOGI(TAG, "Loop timer: Wait MQTT");

			/* Get global temperature, use for all units. */
			uint32_t temp = 0;
			int state = 0;
			char rom_code[17];
			if (xSemaphoreTake(xSemaphoreDS18B20, 10) == pdTRUE) {
			    temp = (ds18b20_state->sensor[0].temperature * 1000);
			    state = (ds18b20_state->sensor[0].error == 0) ? 0:1;
			    strncpy(rom_code, ds18b20_state->sensor[0].rom_code, 17);
			    rom_code[16] = '\0';
        		    xSemaphoreGive(xSemaphoreDS18B20);
    			}

			/* Copy measured data and calculate results */
			if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE) {
			    for (int i = 0; i < 3; i++) {
			    	units[i].temperature = temp;
			    	units[i].temperature_state = state;
				units[i].alarm = 0;
				if (state)
				    units[i].alarm |= ALARM_SYS_TEMPERATURE & ALARM_UNIT_TEMPERATURE;
			    	strncpy(units[i].temperature_rom_code, rom_code, 17);
			    	if (xSemaphoreTake(xSemaphoreADC, 10) == pdTRUE) {
				    units[i].pressure_state = adc_state->Pressure[i].error;
				    units[i].pressure_channel = adc_state->Pressure[i].channel;
				    units[i].pressure_voltage = adc_state->Pressure[i].voltage;
				    if (units[i].pressure_state || units[i].pressure_voltage < 80)
					units[i].alarm |= ALARM_UNIT_PRESSURE;
				    int P = (units[i].pressure_voltage / (adc_state->Batt_voltage / 1000) - units[i].pressure_zero) * 14; // in bar
				    if (P < 0)
				    	P = 0;
				    units[i].pressure = P;
printf("%d volt: %d batt: %d scale: %d  mbar: %d\n", i, units[i].pressure_voltage, adc_state->Batt_voltage, 
		units[i].pressure_voltage / (adc_state->Batt_voltage / 1000) - units[i].pressure_zero, P);
// Moet die echt op 5 volt?
// Verbruik 10 mA
// Setup tijd max 2 mS
				    xSemaphoreGive(xSemaphoreADC);
			    	}
			    }
			    write_units();
			    xSemaphoreGive(xSemaphoreUnits);
			    switch (Main_Loop2) {
				case ML2_USER:	screen_main(); break;
				case ML2_UNIT1:	screen_unit(0); break;
				case ML2_UNIT2: screen_unit(1); break;
				case ML2_UNIT3: screen_unit(2); break;
			    }
			}
		    }
		    break;

		case ML1_WAITCON:
		    if (ready_mqtt())
			Main_Loop1 = ML1_SEND;
		    break;

		case ML1_SEND:
		    ESP_LOGI(TAG, "Loop timer: Send MQTT");
		    publishNode();
		    publishUnits();
		    publishLogs();
		    Main_Loop1 = ML1_WAITACK;
		    break;

		case ML1_WAITACK:
		    if (count_pub == 0) // Wait until all published messages are sent.
			Main_Loop1 = ML1_MQTT_DISCONNECT;
		    break;

		case ML1_MQTT_DISCONNECT:
		    ESP_LOGI(TAG, "Loop timer: Disconnect MQTT");
		    connect_mqtt(false); // Doesn't really disconnect.
                    Main_Loop1 = ML1_DISCONNECT;
		    break;

		case ML1_DISCONNECT:
		    if (! ready_mqtt()) {
			ESP_LOGI(TAG, "Loop timer: WiFi off");
		    	requestWiFi_system(false);
		    	Main_Loop1 = ML1_WIFI_OFF;
		    }
		    break;

		case ML1_WIFI_OFF:
		    if (! ready_WiFi()) {
			ESP_LOGI(TAG, "Loop timer: Done");
			Main_Loop1 = ML1_DONE;
			if (Main_Loop2 == ML2_WIFI)
			    screen_wifi();
		    }
		    break;

		case ML1_DONE:
		    break;
	    }

	    /*
	     * One time actions
	     */
	    if (New_Loop2 != Main_Loop2) {

		Main_Loop2 = New_Loop2;

		switch (Main_Loop2) {
		    case ML2_INIT:
			ESP_LOGI(TAG, "Loop user: Init");
			New_Loop2 = ML2_USER;
			usertimer = INACTIVITY;
			break;

		    case ML2_USER:
			ESP_LOGI(TAG, "Loop user: User mainmenu");
			screen_main();
			break;

		    case ML2_UNIT1:
		    case ML2_UNIT2:
		    case ML2_UNIT3:
			ESP_LOGI(TAG, "Loop user: Unit %d", Main_Loop2 - ML2_UNIT1);
			screen_unit(Main_Loop2 - ML2_UNIT1);
			break;

		    case ML2_WIFI:
			ESP_LOGI(TAG, "Loop user: WiFi");
			screen_wifi();
			break;

		    case ML2_NETWORK:
			ESP_LOGI(TAG, "Loop user: Network");
			screen_network();
			break;

		    case ML2_MQTT:
			ESP_LOGI(TAG, "Loop user: MQTT");
			screen_mqtt();
			break;

		    case ML2_SETUP_MQTT:
			ESP_LOGI(TAG, "Loop user: MQTT setup");
			sprintf(txt, "EDtXt");
			rotary_editer("MQTT demo", txt, "", 16, EDIT_TYPE_TEXT);
			New_Loop2 = ML2_MQTT;
			break;

		    case ML2_UPDATE:
			ESP_LOGI(TAG, "Loop user: Update");
			screen_update();
			break;

		    case ML2_SETUP_UNIT1:
		    case ML2_SETUP_UNIT2:
		    case ML2_SETUP_UNIT3:
			ESP_LOGI(TAG, "Loop user: Setup Unit %d", Main_Loop2 - ML2_SETUP_UNIT1);
			sub = 0;
			screen_unit_setup(Main_Loop2 - ML2_SETUP_UNIT1, sub);
			break;

		    case ML2_ZERO_UNIT1:
		    case ML2_ZERO_UNIT2:
		    case ML2_ZERO_UNIT3:
			ESP_LOGI(TAG, "Loop user: Zero Unit %d", Main_Loop2 - ML2_ZERO_UNIT1);
                        sub = 0;
                        screen_unit_zero(Main_Loop2 - ML2_ZERO_UNIT1, sub);
                        break;

		    case ML2_INACTIVE:
			ESP_LOGI(TAG, "Loop user: Inactive");
			u8g2_SetPowerSave(&u8g2, 1); // powersave display
			New_Loop2 = ML2_DONE;
			break;

		    default:
			break;
		}
	    }

	    /*
	     * Handle rotationg the rotary encoder.
	     */
	    if (Main_Loop2 < ML2_INACTIVE) {
		// If not configured, start configure
		// If configured select first unit
		// Handle screen (first is show measured values)

		// Display per unit. Temp + Pressure + state. Press is setup this sensor.
		// Setup menu: 	Sensors
		//		WiFi
		//		MQTT
		//		System (timers etc)
		//		Update OTA
		//		Return

		// Sensors menu:	Assignments, turn to choose one.
		// Sensors setup menu:	DS18B20 addr	Press is assign
		//			DS18B20 addr

		if (xQueueReceive(event_queue, &event, 250 / portTICK_PERIOD_MS) == pdTRUE) {
		    usertimer = INACTIVITY;
		    switch (Main_Loop2) {
		    	case ML2_USER:		rotate_to_menu(event.state.position, ML2_UNIT1, ML2_USER); break;
		    	case ML2_UNIT1:		rotate_to_menu(event.state.position, ML2_UNIT2, ML2_USER); break;
		    	case ML2_UNIT2:		rotate_to_menu(event.state.position, ML2_UNIT3, ML2_UNIT1); break;
		    	case ML2_UNIT3:		rotate_to_menu(event.state.position, ML2_WIFI, ML2_UNIT2); break;
		    	case ML2_WIFI:		rotate_to_menu(event.state.position, ML2_NETWORK, ML2_UNIT3); break;
		    	case ML2_NETWORK:	rotate_to_menu(event.state.position, ML2_MQTT, ML2_WIFI); break;
		    	case ML2_MQTT:		rotate_to_menu(event.state.position, ML2_UPDATE, ML2_NETWORK); break;
		    	case ML2_UPDATE:	rotate_to_menu(event.state.position, ML2_UPDATE, ML2_MQTT); break;
			case ML2_SETUP_UNIT1:
			case ML2_SETUP_UNIT2:
			case ML2_SETUP_UNIT3:	if (rotate_to_sub(event.state.position, 0, 3, &sub))
						    screen_unit_setup(Main_Loop2 - ML2_SETUP_UNIT1, sub);
						break;
			case ML2_ZERO_UNIT1:
			case ML2_ZERO_UNIT2:
			case ML2_ZERO_UNIT3:	if (rotate_to_sub(event.state.position, 0, 1, &sub))
						    screen_unit_zero(Main_Loop2 - ML2_ZERO_UNIT1, sub);
						break;
			default:
		    				ESP_LOGI(TAG, "Event: position %d, direction %s", event.state.position,
                                		event.state.direction ? (event.state.direction == ROTARY_ENCODER_DIRECTION_CLOCKWISE ? "CW":"CCW"):"NOT_SET");
		    }
                } else {
                    // Poll current position and direction
//                    rotary_encoder_state_t state = { 0 };
//                    ESP_ERROR_CHECK(rotary_encoder_get_state(&rinfo, &state));

//                    ESP_LOGI(TAG, "Poll: position %d, direction %s timer %d", state.position,
//                    state.direction ? (state.direction == ROTARY_ENCODER_DIRECTION_CLOCKWISE ? "CW" : "CCW") : "NOT_SET", usertimer);
		    if (usertimer) {
			usertimer--;
			if ((usertimer % 240) == 0) { // Each minute
			    ESP_LOGI(TAG, "usertimer %d", usertimer);
			    if (Main_Loop1 == ML1_DONE)
				Main_Loop1 = ML1_INIT;
			}
		    } else
			New_Loop2 = ML2_INACTIVE;
                }
	    }

	    /*
	     * Handle pressed rotary button.
	     */
	    if (PushDuration) {
		int idx = 0;
	    	switch (Main_Loop2) {
		    case ML2_UNIT1:
		    case ML2_UNIT2:
		    case ML2_UNIT3:
			New_Loop2 = ML2_SETUP_UNIT1 + (Main_Loop2 - ML2_UNIT1);
			break;

		    case ML2_SETUP_UNIT1:
		    case ML2_SETUP_UNIT2:
		    case ML2_SETUP_UNIT3:
			idx = Main_Loop2 - ML2_SETUP_UNIT1;
			if (sub == 0) {
			    if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE) {
				if (units[idx].mode)
				    units[idx].mode = 0;
				else
				    units[idx].mode = 1;
				write_units();
                            	xSemaphoreGive(xSemaphoreUnits);
			    }
			    screen_unit_setup(idx, sub);
			    if (Main_Loop1 == ML1_DONE)
				Main_Loop1 = ML1_INIT;
			}
			if (sub == 1)
			    New_Loop2 = ML2_ZERO_UNIT1 + idx;
			if (sub == 3)
			    New_Loop2 = ML2_UNIT1 + idx;
			printf("unit setup sub %d  new %d  idx %d\n", sub, New_Loop2, idx);
			break;

		    case ML2_ZERO_UNIT1:
		    case ML2_ZERO_UNIT2:
		    case ML2_ZERO_UNIT3:
			idx = Main_Loop2 - ML2_ZERO_UNIT1;
                        if (sub == 0) {
			    if (xSemaphoreTake(xSemaphoreUnits, 25) == pdTRUE &&
			      xSemaphoreTake(xSemaphoreADC, 10) == pdTRUE &&
			      adc_state->Pressure[idx].voltage > 165 &&
			      adc_state->Pressure[idx].voltage < 660) {
				units[idx].pressure_zero = adc_state->Pressure[idx].voltage / (adc_state->Batt_voltage / 1000);
				write_units();
				xSemaphoreGive(xSemaphoreADC);
                                xSemaphoreGive(xSemaphoreUnits);
				screen_unit_zero(idx, sub);
				if (Main_Loop1 == ML1_DONE)
                            	    Main_Loop1 = ML1_INIT;
			    }
			}
			if (sub == 1) {
			    New_Loop2 = ML2_SETUP_UNIT1 + idx;
			    sub = 1;
			}
			printf("unit zero sub %d  new %d  idx %d\n", sub, New_Loop2, idx);
			break;

		    case ML2_MQTT:
			New_Loop2 = ML2_SETUP_MQTT;
			break;

		    default:
			break;
	    	}
	    	PushDuration = 0;
	    }

	    if (Main_Loop1 == ML1_DONE && Main_Loop2 == ML2_DONE)
	    	break;

	    vTaskDelay(10 / portTICK_PERIOD_MS);
	}

	ESP_LOGI(TAG, "Entering deep sleep");
    	gettimeofday(&sleep_enter_time, NULL);
	esp_deep_sleep_start();

	Main_Loop1 = ML1_INIT;
	New_Loop2 = ML2_INIT;
    }

}