/*
 * diot_util - diagnostic tool for DIOT system board
 * 
 * Author: Adam Wujek for CERN
 * Copyright CERN 2021
 * 
 */


#include <stdio.h>
#include <string.h>
#include <gpiod.h>
#include <unistd.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <stdint.h>

#include <readline/readline.h>
#include <readline/history.h>
#include <sensors/sensors.h>
#include <sensors/error.h>
#include "util.h"
#include "term.h"
#include "diot_util.h"
#include "diot_util_error.h"
#include "diot_util_gpio.h"
#include "diot_util_pts.h"
#include "diot_util_i2c.h"

#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))
#define ARRAY_SIZE(arr)     (sizeof(arr) / sizeof((arr)[0]))
#define ALARM_CHECK(x) x ? COLOR_RED: ""
#define ALARM_OFF(x) x ? COLOR_OFF: ""

struct hw_sensor {
    char *short_name;
    char *chip_name;
    char *sensor_name; /* if null take max value from all sensors in the chip */
    sensors_feature_type sensor_type;
    double read_val;
    double min;
    double max;
    double crit_min;
    double crit_max;
    double emerg;
    int alarm;
};

#define DECLARE_DIOT_SENSOR(_short_name, _chip_name, _sensor_name) \
	{ .short_name = _short_name, \
	    .chip_name = _chip_name, \
	    .sensor_name = _sensor_name }

/* List of Temperature sensors */
static struct hw_sensor hw_sensors_temp_list[] = {
    DECLARE_DIOT_SENSOR("FPGA",  "max6639-i2c-" MAX6639_I2C "-" MAX6639_I2C_ADDR,      "Temp FPGA"),
    DECLARE_DIOT_SENSOR("FMC",   "lm75a-i2c-" LM75_I2C "-" LM75_1_I2C_ADDR,            "Temp FMC"),
    DECLARE_DIOT_SENSOR("DDR",   "lm75a-i2c-" LM75_I2C "-" LM75_2_I2C_ADDR,            "Temp DDR"),
    DECLARE_DIOT_SENSOR("Pow M", "lm75a-i2c-" LM75_I2C "-" LM75_3_I2C_ADDR,            "Temp Power Management"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P0V85 Temp"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8_AUX  Temp"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2 Temp"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8 Temp"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2_PSPLL LDO Temp"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 P3V3 Temp"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V8 Temp"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_0V9 Temp"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V2 Temp"),
    DECLARE_DIOT_SENSOR("PSU1",  "D57500520-i2c-" PSU_I2C "-" PSU1_I2C_ADDR,           "PSU1 Temp1"),
    DECLARE_DIOT_SENSOR("PSU1",  "D57500520-i2c-" PSU_I2C "-" PSU1_I2C_ADDR,           "PSU1 Temp2"),
    DECLARE_DIOT_SENSOR("PSU2",  "D57500520-i2c-" PSU_I2C "-" PSU2_I2C_ADDR,           "PSU2 Temp1"),
    DECLARE_DIOT_SENSOR("PSU2",  "D57500520-i2c-" PSU_I2C "-" PSU2_I2C_ADDR,           "PSU2 Temp2"),
    DECLARE_DIOT_SENSOR("FANTRAY", "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,        "FANTRAY Temp"),
    DECLARE_DIOT_SENSOR(NULL, NULL, NULL)
};

/* List of voltage, current, power (vcp) sensors */
static struct hw_sensor hw_sensors_vcp_list[] = {
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 Input Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 Input Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 Input Voltage"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P0V85 Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P0V85 Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P0V85 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2 Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2 Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2_PSPLL LDO Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2_PSPLL LDO Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V2_PSPLL LDO Voltage"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8_AUX Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8_AUX Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8_AUX Voltage"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8 Current"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8 Power"),
    DECLARE_DIOT_SENSOR("IRPS1", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_1_I2C_ADDR, "IRPS1 P1V8 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 Input Current"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 Input Power"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 Input Voltage"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_0V9 Current"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_0V9 Power"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_0V9 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V2 Current"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V2 Power"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V2 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V8 Current"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V8 Power"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 MGT_1V8 Voltage"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 P3V3 Current"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 P3V3 Power"),
    DECLARE_DIOT_SENSOR("IRPS2", "irps5401-i2c-" IRPS5401_I2C "-" IRPS5401_2_I2C_ADDR, "IRPS2 P3V3 Voltage"),

    DECLARE_DIOT_SENSOR("PSU1",  "D57500520-i2c-" PSU_I2C "-" PSU1_I2C_ADDR,           "PSU1 12V Voltage"),
    DECLARE_DIOT_SENSOR("PSU1",  "D57500520-i2c-" PSU_I2C "-" PSU1_I2C_ADDR,           "PSU1 12V Current"),
    DECLARE_DIOT_SENSOR("PSU1",  "D57500520-i2c-" PSU_I2C "-" PSU1_I2C_ADDR,           "PSU1 12V Power"),
    DECLARE_DIOT_SENSOR("PSU2",  "D57500520-i2c-" PSU_I2C "-" PSU2_I2C_ADDR,           "PSU2 12V Voltage"),
    DECLARE_DIOT_SENSOR("PSU2",  "D57500520-i2c-" PSU_I2C "-" PSU2_I2C_ADDR,           "PSU2 12V Current"),
    DECLARE_DIOT_SENSOR("PSU2",  "D57500520-i2c-" PSU_I2C "-" PSU2_I2C_ADDR,           "PSU2 12V Power"),
    DECLARE_DIOT_SENSOR(NULL, NULL, NULL)
};

/* List of Fan sensors */
static struct hw_sensor hw_sensors_monimod_list[] = {
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FANTRAY 12V Voltage"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FANTRAY 5V Voltage"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN1 Current"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN2 Current"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN3 Current"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN1 RPM Slots S+P1-4"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN2 RPM Slots P5-8"),
    DECLARE_DIOT_SENSOR("FANTRAY",   "monimod-i2c-" PSU_I2C "-" FANTRAY_I2C_ADDR,      "FAN3 RPM Power Supp"),
    DECLARE_DIOT_SENSOR(NULL, NULL, NULL)
};

struct sensor_short {
    char *short_name;
    char *sensor_name; /* if null take max value from all sensors in the chip */
};

#define DECLARE_SENSOR_SHORT(_short_name, _sensor_name) \
	{ .short_name = _short_name, \
	    .sensor_name = _sensor_name }

/* Lists used to print sensors */
static struct sensor_short temp_sensors_list[] = {
    DECLARE_SENSOR_SHORT("FPGA", "Temp FPGA"),
    DECLARE_SENSOR_SHORT("FMC", "Temp FMC"),
    DECLARE_SENSOR_SHORT("DDR", "Temp DDR"),
    DECLARE_SENSOR_SHORT("Pow M", "Temp Power Management"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short temp_sensors_psu1_list[] = {
    DECLARE_SENSOR_SHORT("PSU1", "PSU1 Temp1"),
    DECLARE_SENSOR_SHORT("PSU1", "PSU1 Temp2"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short temp_sensors_psu2_list[] = {
    DECLARE_SENSOR_SHORT("PSU2", "PSU2 Temp1"),
    DECLARE_SENSOR_SHORT("PSU2", "PSU2 Temp2"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short temp_sensors_irps1_list[] = {
    DECLARE_SENSOR_SHORT("IRPS1", "IRPS1 P0V85 Temp"),
    DECLARE_SENSOR_SHORT("IRPS1", "IRPS1 P1V8_AUX  Temp"),
    DECLARE_SENSOR_SHORT("IRPS1", "IRPS1 P1V2 Temp"),
    DECLARE_SENSOR_SHORT("IRPS1", "IRPS1 P1V8 Temp"),
    DECLARE_SENSOR_SHORT("IRPS1", "IRPS1 P1V2_PSPLL LDO Temp"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short temp_sensors_irps2_list[] = {
    DECLARE_SENSOR_SHORT("IRPS2", "IRPS2 P3V3 Temp"),
    DECLARE_SENSOR_SHORT("IRPS2", "IRPS2 MGT_1V8 Temp"),
    DECLARE_SENSOR_SHORT("IRPS2", "IRPS2 MGT_0V9 Temp"),
    DECLARE_SENSOR_SHORT("IRPS2", "IRPS2 MGT_1V2 Temp"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short temp_sensors_monimod_list[] = {
    DECLARE_SENSOR_SHORT("FANTRAY", "FANTRAY Temp"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

static struct sensor_short sensors_monimod_list[] = {
    DECLARE_SENSOR_SHORT("FANTRAY", "FANTRAY 12V Voltage"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FANTRAY 5V Voltage"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN1 Current"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN2 Current"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN3 Current"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN1 RPM Slots S+P1-4"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN2 RPM Slots P5-8"),
    DECLARE_SENSOR_SHORT("FANTRAY", "FAN3 RPM Power Supp"),
    DECLARE_SENSOR_SHORT(NULL, NULL),
};

struct short_vcp { /* vcp = Voltage, Current, Power */
    char *short_name;
    char *voltage_sensor_name;
    char *current_sensor_name;
    char *power_sensor_name;
};

#define DECLARE_SHORT_VCP(_short_name, _voltage_sensor_name, \
			  _current_sensor_name, _power_sensor_name) \
	{ .short_name = _short_name, \
	    .voltage_sensor_name = _voltage_sensor_name, \
	    .current_sensor_name = _current_sensor_name, \
	    .power_sensor_name = _power_sensor_name, \
	}

/* Lists used to print voltage, current, power sensors */
static struct short_vcp irps1_entries[] = {
    DECLARE_SHORT_VCP("Input",          "IRPS1 Input Voltage",          "IRPS1 Input Current",          "IRPS1 Input Power"),
    DECLARE_SHORT_VCP("P0V85",          "IRPS1 P0V85 Voltage",          "IRPS1 P0V85 Current",          "IRPS1 P0V85 Power"),
    DECLARE_SHORT_VCP("P1V8_AUX",       "IRPS1 P1V8_AUX Voltage",       "IRPS1 P1V8_AUX Current",       "IRPS1 P1V8_AUX Power"),
    DECLARE_SHORT_VCP("P1V2",           "IRPS1 P1V2 Voltage",           "IRPS1 P1V2 Current",           "IRPS1 P1V2 Power"),
    DECLARE_SHORT_VCP("P1V8",           "IRPS1 P1V8 Voltage",           "IRPS1 P1V8 Current",           "IRPS1 P1V8 Power"),
    DECLARE_SHORT_VCP("P1V2_PSPLL LDO", "IRPS1 P1V2_PSPLL LDO Voltage", "IRPS1 P1V2_PSPLL LDO Current", "IRPS1 P1V2_PSPLL LDO Power"),
    DECLARE_SHORT_VCP(NULL, NULL, NULL, NULL),
};

static struct short_vcp irps2_entries[] = {
    DECLARE_SHORT_VCP("Input",          "IRPS2 Input Voltage",          "IRPS2 Input Current",          "IRPS2 Input Power"),
    DECLARE_SHORT_VCP("P3V3",           "IRPS2 P3V3 Voltage",           "IRPS2 P3V3 Current",           "IRPS2 P3V3 Power"),
    DECLARE_SHORT_VCP("MGT_1V8",        "IRPS2 MGT_1V8 Voltage",        "IRPS2 MGT_1V8 Current",        "IRPS2 MGT_1V8 Power"),
    DECLARE_SHORT_VCP("MGT_0V9",        "IRPS2 MGT_0V9 Voltage",        "IRPS2 MGT_0V9 Current",        "IRPS2 MGT_0V9 Power"),
    DECLARE_SHORT_VCP("MGT_1V2",        "IRPS2 MGT_1V2 Voltage",        "IRPS2 MGT_1V2 Current",        "IRPS2 MGT_1V2 Power"),
    DECLARE_SHORT_VCP(NULL, NULL, NULL, NULL),
};

static struct short_vcp psu1_entries[] = {
    DECLARE_SHORT_VCP("PSU1",           "PSU1 12V Voltage",             "PSU1 12V Current",             "PSU1 12V Power"),
    DECLARE_SHORT_VCP(NULL, NULL, NULL, NULL),
};

static struct short_vcp psu2_entries[] = {
    DECLARE_SHORT_VCP("PSU2",           "PSU2 12V Voltage",             "PSU2 12V Current",             "PSU2 12V Power"),
    DECLARE_SHORT_VCP(NULL, NULL, NULL, NULL),
};

struct sensor_alarms {
    sensors_subfeature_type alarm_type;
    char *alarm_msg;
};

#define DECLARE_ALARM(_alarm_type, _alarm_msg) \
	{ .alarm_type = _alarm_type, \
	    .alarm_msg = _alarm_msg }

/* List of sensors' alarms to be checked */
static struct sensor_alarms alarms_list[] = {
    DECLARE_ALARM(SENSORS_SUBFEATURE_TEMP_ALARM,           "Temperature Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_TEMP_MAX_ALARM,       "Temperature MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_TEMP_CRIT_ALARM,      "Temperature CRITICAL Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_TEMP_EMERGENCY_ALARM, "Temperature EMERGENCY Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_IN_ALARM,             "Voltage Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_IN_MIN_ALARM,         "Voltage MIN Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_IN_MAX_ALARM,         "Voltage MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_IN_LCRIT_ALARM,       "Voltage CRITICAL MIN Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_IN_CRIT_ALARM,        "Voltage CRITICAL MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_CURR_ALARM,           "Current Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_CURR_MIN_ALARM,       "Current MIN Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_CURR_MAX_ALARM,       "Current MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_CURR_LCRIT_ALARM,     "Current CRITICAL MIN Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_CURR_CRIT_ALARM,      "Current CRITICAL MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_POWER_ALARM,          "Power Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_POWER_MAX_ALARM,      "Power MAX Alarm"),
    DECLARE_ALARM(SENSORS_SUBFEATURE_POWER_CRIT_ALARM,     "Power CRITICAL MAX Alarm"),
};

/* Prototypes */
static double get_value(const sensors_chip_name *name,
			const sensors_subfeature *sub);
static int get_input_value(const sensors_chip_name *name,
			   const sensors_subfeature *sub,
			   double *val);
static void check_psu_state(void);

static int check_alarms(char *chip_name, char *sensor_name,
		  const sensors_chip_name *chip_selected,
		  const sensors_subfeature *sf)
{
    int i;

    for (i = 0; i < ARRAY_SIZE(alarms_list); i++) {
	if (alarms_list[i].alarm_type == sf->type) {
	    if (get_value(chip_selected, sf)) {
		diot_error_add("ALARM: chip %s, sensor %s, message: %s\n",
			       chip_name, sensor_name,
			       alarms_list[i].alarm_msg);
		/* No need to check more */
		return 1;
	    }

	    /* No need to check more */
	    return 0;
	}
    }
    return 0;
}

static void get_sensor_values(struct hw_sensor *sensor_to_read)
{
    sensors_chip_name chip;
    const sensors_chip_name *chip_selected;
    const sensors_feature *feature;
    const sensors_subfeature *sf;
    int i_feature;
    char *label = NULL;
    int sf_i;

    /* clear sensor's values */
    sensor_to_read->read_val = NAN;
    sensor_to_read->min = NAN;
    sensor_to_read->max = NAN;
    sensor_to_read->crit_min = NAN;
    sensor_to_read->crit_max = NAN;
    sensor_to_read->emerg = NAN;
    sensor_to_read->alarm = 0;

    if (!sensor_to_read->chip_name) {
        diot_error_add("ERROR: Chip name empty\n");
	return;
    }
    if (!sensor_to_read->sensor_name) {
        diot_error_add("ERROR: Sensor name empty, chip name %s\n",
		       sensor_to_read->chip_name);
	return;
    }

    if (sensors_parse_chip_name(sensor_to_read->chip_name, &chip)) {
        diot_error_add("ERROR: Chip %s not available (parse error?)\n",
		       sensor_to_read->chip_name);
	return;
    }

    int tmp = 0;
    chip_selected = sensors_get_detected_chips(&chip, &tmp);
    if (!chip_selected) {
        diot_error_add("ERROR: Chip %s not available\n",
		       sensor_to_read->chip_name);
	sensors_free_chip_name(&chip);
	return;
    }

    i_feature = 0;
    while ((feature = sensors_get_features(chip_selected, &i_feature))) {
	if (!(label = sensors_get_label(chip_selected, feature))) {
	    printf("%6s |", "l ERR");
	    diot_error_add("ERROR: Can't get label of sensor %s!\n",
		    feature->name);
	    continue;
	}
	if (strcmp(label, sensor_to_read->sensor_name)) {
	    /* not this sensor */
	    free(label);
	    label = NULL;
	    continue;
	}
	/* check type? */
	sf_i = 0;
	while ((sf = sensors_get_all_subfeatures(chip_selected, feature, &sf_i))) {
	    switch (sf->type) {
	    case SENSORS_SUBFEATURE_IN_INPUT:
	    case SENSORS_SUBFEATURE_FAN_INPUT:
	    case SENSORS_SUBFEATURE_TEMP_INPUT:
	    case SENSORS_SUBFEATURE_POWER_INPUT:
	    case SENSORS_SUBFEATURE_ENERGY_INPUT:
	    case SENSORS_SUBFEATURE_CURR_INPUT:
	    case SENSORS_SUBFEATURE_HUMIDITY_INPUT:
		get_input_value(chip_selected, sf, &sensor_to_read->read_val);
		break;

	    case SENSORS_SUBFEATURE_IN_MIN:
	    case SENSORS_SUBFEATURE_FAN_MIN:
	    case SENSORS_SUBFEATURE_TEMP_MIN:
	    case SENSORS_SUBFEATURE_CURR_MIN:
		get_input_value(chip_selected, sf, &sensor_to_read->min);
		break;

	    case SENSORS_SUBFEATURE_IN_MAX:
	    case SENSORS_SUBFEATURE_FAN_MAX:
	    case SENSORS_SUBFEATURE_TEMP_MAX:
	    case SENSORS_SUBFEATURE_POWER_MAX:
	    case SENSORS_SUBFEATURE_CURR_MAX:
		get_input_value(chip_selected, sf, &sensor_to_read->max);
		break;

	    case SENSORS_SUBFEATURE_IN_LCRIT:
	    case SENSORS_SUBFEATURE_TEMP_LCRIT:
	    case SENSORS_SUBFEATURE_CURR_LCRIT:
		get_input_value(chip_selected, sf, &sensor_to_read->crit_min);
		break;

	    case SENSORS_SUBFEATURE_IN_CRIT:
	    case SENSORS_SUBFEATURE_TEMP_CRIT:
	    case SENSORS_SUBFEATURE_POWER_CRIT:
	    case SENSORS_SUBFEATURE_CURR_CRIT:
		get_input_value(chip_selected, sf, &sensor_to_read->crit_max);
		break;

	    case SENSORS_SUBFEATURE_TEMP_EMERGENCY:
		get_input_value(chip_selected, sf, &sensor_to_read->emerg);
		break;

	    default:
		/* check if subfeature is an alarm */
		sensor_to_read->alarm += check_alarms(sensor_to_read->chip_name,
						      label, chip_selected, sf);
		break;
	    }
	}
	free(label);
	label = NULL;

	break;
    }
    sensors_free_chip_name(&chip);
}

static struct hw_sensor * find_hw_sensor(char *sensor_name,
					 struct hw_sensor *hw_sensors_list)
{
    struct hw_sensor *sensor_p = hw_sensors_list;

    for (; sensor_p->chip_name; sensor_p++) {
	if (!strcmp(sensor_name, sensor_p->sensor_name)) {
	    /* sensor's name should be uniq */
	    return sensor_p;
	}
    }
    return NULL;
}

static void read_one_sensor(char *sensor_to_read,
			    struct hw_sensor *hw_sensors_list)
{
    struct hw_sensor *sensor_p;

    sensor_p = find_hw_sensor(sensor_to_read, hw_sensors_list);

    if (sensor_p)
	get_sensor_values(sensor_p);
}



static void read_sensors(struct sensor_short *sensors_to_read_list,
			 struct hw_sensor *hw_sensors_list)
{
    struct sensor_short *sensor_to_read = sensors_to_read_list;

    for (; sensor_to_read->short_name; sensor_to_read++) {
	read_one_sensor(sensor_to_read->sensor_name, hw_sensors_list);
    }
}

static void read_sensors_vcp(struct short_vcp *sensors_to_read_list,
			     struct hw_sensor *hw_sensors_list)
{
    struct short_vcp *sensor_to_read = sensors_to_read_list;

    for (; sensor_to_read->short_name; sensor_to_read++) {
	read_one_sensor(sensor_to_read->voltage_sensor_name, hw_sensors_list);
	read_one_sensor(sensor_to_read->current_sensor_name, hw_sensors_list);
	read_one_sensor(sensor_to_read->power_sensor_name, hw_sensors_list);
    }
}


void read_all_sensors(void)
{
    int expected_errors;

    /* Read sensors */
    read_sensors(temp_sensors_list, hw_sensors_temp_list);
    read_sensors(temp_sensors_irps1_list, hw_sensors_temp_list);
    read_sensors(temp_sensors_irps2_list, hw_sensors_temp_list);

    /* Read VCP sensors */
    read_sensors_vcp(irps1_entries, hw_sensors_vcp_list);
    read_sensors_vcp(irps2_entries, hw_sensors_vcp_list);

    /* To detect whether PSU1, PSU2 and FAN are present some tricks are needed.
     * It is done in several steps:
     * 1. Read Temperature sensors (ignore error messages)
     * 2. Check if PSU/FAN were removed/inserted (keep error messages)
     * 3. Read all sensors from a given unit (e.g. PSU1)
     * 4. If all sensors were not read correctly means the unit is not inserted
     *    In this case print a single error message.
     * Repeat steps 3-4 for other units.
     */
    diot_error_add_to_secondary_buff();
    diot_error_clear();

    read_sensors(temp_sensors_psu1_list, hw_sensors_temp_list);
    read_sensors(temp_sensors_psu2_list, hw_sensors_temp_list);
    read_sensors(temp_sensors_monimod_list, hw_sensors_temp_list);

    /* Ignore errors from above */
    diot_error_clear();

    /* 2. Check if PSU/FAN were removed/inserted */
    check_psu_state();

    /* Copy errors collected during check_psu_state */
    diot_error_add_to_secondary_buff();
    diot_error_clear();


    /* 3. Read PSU1 sensors once more */
    read_sensors(temp_sensors_psu1_list, hw_sensors_temp_list);
    read_sensors_vcp(psu1_entries, hw_sensors_vcp_list);

    /* number of Temperature sensors */
    expected_errors = ARRAY_SIZE(temp_sensors_psu1_list) - 1;
    /* number of VCP sensors */
    expected_errors += (ARRAY_SIZE(psu1_entries) - 1) * 3;

    if (diot_error_buff_n_errors() == expected_errors) {
	diot_error_clear();
	diot_error_add("WARNING: PSU1 not present\n");
    }

    diot_error_add_to_secondary_buff();
    diot_error_clear();


    /* 3. Read PSU2 sensors once more */
    read_sensors(temp_sensors_psu2_list, hw_sensors_temp_list);
    read_sensors_vcp(psu2_entries, hw_sensors_vcp_list);

    /* number of Temperature sensors */
    expected_errors = ARRAY_SIZE(temp_sensors_psu2_list) - 1;
    /* number of VCP sensors */
    expected_errors += (ARRAY_SIZE(psu2_entries) - 1) * 3;

    if (diot_error_buff_n_errors() == expected_errors) {
	diot_error_clear();
	diot_error_add("WARNING: PSU2 not present\n");
    }

    diot_error_add_to_secondary_buff();
    diot_error_clear();


    /* 3. Read FAN sensors once more */
    read_sensors(temp_sensors_monimod_list, hw_sensors_temp_list);
    read_sensors(sensors_monimod_list, hw_sensors_monimod_list);

    /* number of Temperature sensors */
    expected_errors = ARRAY_SIZE(temp_sensors_monimod_list) - 1;
    /* number of other sensors */
    expected_errors += ARRAY_SIZE(sensors_monimod_list) - 1;

    if (diot_error_buff_n_errors() == expected_errors) {
	diot_error_clear();
	diot_error_add("WARNING: FAN module not present\n");
    }

    diot_error_add_to_secondary_buff();
    diot_error_clear();

    /* Copy back all error messages to the main buffer */
    diot_error_move_from_secondary_buff();
}


static double get_value(const sensors_chip_name *name,
			const sensors_subfeature *sub)
{
	double val;
	int err;

	err = sensors_get_value(name, sub->number, &val);
	if (err) {
		diot_error_add("ERROR: Can't get value of subfeature %s: %s\n",
			       sub->name, sensors_strerror(err));
		val = 0;
	}
	return val;
}

/* A variant for input values, where we want to handle errors gracefully */
static int get_input_value(const sensors_chip_name *name,
			   const sensors_subfeature *sub,
			   double *val)
{
	int err;

	err = sensors_get_value(name, sub->number, val);
	if (err && err != -SENSORS_ERR_ACCESS_R) {
		diot_error_add("ERROR: Can't get value of subfeature %s: %s\n",
			       sub->name, sensors_strerror(err));
	}
	return err;
}

static int psu_verify_state(int psu, char *psu_temp_sensor_name)
{
    struct hw_sensor *sensor_p;

    /* No need to check if PSU is not present */
    if (!read_psu_presence(psu))
	return 1;

    sensor_p = find_hw_sensor(psu_temp_sensor_name, hw_sensors_temp_list);

    if (!sensor_p) {
	diot_error_add("BUG: sensor name %s not found in "
		       "hw_sensors_temp_list\n",
		       psu_temp_sensor_name);

	return 1;
    }

    if (isnan(sensor_p->read_val)) {
	/* PSU is present, but read_val is NAN, looks like a problem */
	diot_error_add("ERROR: Unable to read %s from a present PSU, try to "
		       "reset i2c-" PSU_I2C "\n",
		       psu_temp_sensor_name);
	return 0;
    }

    return 1;
}

static void check_psu_state(void)
{
    if (!psu_verify_state(1, "PSU1 Temp1")
	|| !psu_verify_state(2, "PSU2 Temp1")
    ) {
	if (restore_i2c())
	    diot_error_add("WARNING: Reset of i2c-" PSU_I2C " done\n");

	/* let libsensors know that they might be new sensors available */
	sensors_cleanup();
	sensors_init(NULL);
    }
}

static void print_short_temp_hline(int n)
{
    int i;

    for (i = 0; i < n; i++) {
	printf("-------+");
    }
}

static void print_short_vcp_single_sensor(char *short_name, char *sensor_name,
					  struct hw_sensor *hw_sensors_list)
{
    struct hw_sensor *sensor_p;

    sensor_p = find_hw_sensor(sensor_name, hw_sensors_list);

    if (sensor_p) {
	if (isnan(sensor_p->read_val))
	    printf("%6s |%7s |", "N/A", "N/A");
	else {
	    if (isnan(sensor_p->read_val))
		printf("%6s |%7s |", "N/A", "N/A");
	    else {
		printf("%s%6.2f%s |", sensor_p->alarm ? COLOR_RED : "",
		       sensor_p->read_val, sensor_p->alarm ? COLOR_OFF : "");
		printf("%s%7s%s |", sensor_p->alarm ? COLOR_RED : "",
		       sensor_p->alarm ? "ALARM" : "OK",
		       sensor_p->alarm ? COLOR_OFF : "");
	    }
	}
    } else {
	diot_error_add("BUG: sensor %s not on the sensors list! %s\n",
		       short_name, sensor_name);
	printf("%s%6s%s |%s%7s%s |", COLOR_RED, "ERROR", COLOR_OFF, COLOR_RED, "ERROR", COLOR_OFF);
    }
}

static void print_short_vcp(struct short_vcp *vcp_list,
				    struct hw_sensor *hw_sensors_list)
{
    struct short_vcp *vcp_to_print = vcp_list;

    for (; vcp_to_print->short_name; vcp_to_print++) {
	printf("%14s |", vcp_to_print->short_name);
	print_short_vcp_single_sensor(vcp_to_print->short_name,
				      vcp_to_print->voltage_sensor_name,
				      hw_sensors_list);
	print_short_vcp_single_sensor(vcp_to_print->short_name,
				      vcp_to_print->current_sensor_name,
				      hw_sensors_list);
	print_short_vcp_single_sensor(vcp_to_print->short_name,
				      vcp_to_print->power_sensor_name,
				      hw_sensors_list);
	printf("\n");
    }
}


static void print_short_temp_print_values(struct sensor_short *sensors_print_list,
					  struct hw_sensor *hw_sensors_list)
{
    struct sensor_short *sensor_to_read = sensors_print_list;
    struct hw_sensor *sensor_p;

    for (; sensor_to_read->short_name; sensor_to_read++) {
	sensor_p = find_hw_sensor(sensor_to_read->sensor_name, hw_sensors_list);

	if (sensor_p) {
	    if (isnan(sensor_p->read_val))
		printf("%6s |", "N/A");
	    else
		printf("%s%6.1f%s |", sensor_p->alarm ? COLOR_RED : "",
		       sensor_p->read_val,
		       sensor_p->alarm ? COLOR_OFF : "");
	} else {
	    diot_error_add("BUG: sensor %s not on the sensors list!\n",
			   sensor_to_read->short_name);
	    printf("%s%6s%s |", COLOR_RED, "ERROR", COLOR_OFF);
	}
    }
}

static double get_max_temp(struct sensor_short *sensors_print_list,
				struct hw_sensor *hw_sensors_list, int *alarm)
{
	struct sensor_short *sensor_to_read = sensors_print_list;
	struct hw_sensor *sensor_p;
	double max_val = NAN;
	int temp_alarm = 0;

	for (; sensor_to_read->short_name; sensor_to_read++) {

		sensor_p = find_hw_sensor(sensor_to_read->sensor_name, hw_sensors_list);

		if (sensor_p) {
			if (!isnan(sensor_p->read_val)) {
				temp_alarm += sensor_p->alarm;
				max_val = MAX(max_val, sensor_p->read_val);
			}
		} else {
			diot_error_add("BUG: sensor %s not on the sensors list!\n",
					sensor_to_read->short_name);
		}
	}

	if (alarm)
		*alarm = temp_alarm;

	return max_val;
}

static void print_short_max_temp(struct sensor_short *sensors_print_list,
				 struct hw_sensor *hw_sensors_list)
{
    double max_val = NAN;
    int alarm = 0;

    max_val = get_max_temp(sensors_print_list, hw_sensors_list, &alarm);

    if (isnan(max_val))
	printf("%6s |", "N/A");
    else
	printf("%s%6.1f%s |", alarm ? COLOR_RED : "", max_val,
	       alarm ? COLOR_OFF : "");
}


static void print_short_temp(void)
{
    int i;

    printf("Temperature sensors\n");
    /* Print line */
    print_short_temp_hline(ARRAY_SIZE(temp_sensors_list) - 1);
    print_short_temp_hline(2); /* IRPSs */
    print_short_temp_hline(2); /* PSUs */
    print_short_temp_hline(1); /* FAN */
    printf("\n");

    /* Print sensors' names */
    for (i = 0; temp_sensors_list[i].sensor_name; i++) {
	printf("%6s |", temp_sensors_list[i].short_name);
    }
    printf("%6s |", "IRPS1");
    printf("%6s |", "IRPS2");
    printf("%6s |", "PSU1");
    printf("%6s |", "PSU2");
    printf("%6s |", "FANTR");
    printf("\n");

    /* Print line */
    print_short_temp_hline(ARRAY_SIZE(temp_sensors_list) - 1);
    print_short_temp_hline(2); /* IRPSs */
    print_short_temp_hline(2); /* PSUs */
    print_short_temp_hline(1); /* FAN */
    printf("\n");

    /* Print sensors' values */
    print_short_temp_print_values(temp_sensors_list, hw_sensors_temp_list);
    print_short_max_temp(temp_sensors_irps1_list, hw_sensors_temp_list);
    print_short_max_temp(temp_sensors_irps2_list, hw_sensors_temp_list);
    print_short_max_temp(temp_sensors_psu1_list, hw_sensors_temp_list);
    print_short_max_temp(temp_sensors_psu2_list, hw_sensors_temp_list);
    print_short_max_temp(temp_sensors_monimod_list, hw_sensors_temp_list);
    printf("\n");

    /* Print line */
    print_short_temp_hline(ARRAY_SIZE(temp_sensors_list) - 1);
    print_short_temp_hline(2); /* IRPSs */
    print_short_temp_hline(2); /* PSUs */
    print_short_temp_hline(1); /* FAN */
    printf("\n");
}

static void print_irps_short(void)
{
    printf("Voltage/Current/Power readouts:\n");
    printf("---------------+----------------+----------------+----------------+\n");
    printf("IRPS1          |   Voltage (V)  |   Current (A)  |    Power (W)   |\n");
    printf("               |  read | status |  read | status |  read | status |\n");
    printf("---------------+-------+--------+-------+--------+-------+--------+\n");
    print_short_vcp(irps1_entries, hw_sensors_vcp_list);
    printf("---------------+----------------+----------------+----------------+\n");
    printf("IRPS2          |   Voltage (V)  |   Current (A)  |    Power (W)   |\n");
    printf("               |  read | status |  read | status |  read | status |\n");
    printf("---------------+-------+--------+-------+--------+-------+--------+\n");
    print_short_vcp(irps2_entries, hw_sensors_vcp_list);
    printf("---------------+----------------+----------------+----------------+\n");
    printf("PSU            |   Voltage (V)  |   Current (A)  |    Power (W)   |\n");
    printf("               |  read | status |  read | status |  read | status |\n");
    printf("---------------+-------+--------+-------+--------+-------+--------+\n");
    print_short_vcp(psu1_entries, hw_sensors_vcp_list);
    print_short_vcp(psu2_entries, hw_sensors_vcp_list);
    printf("---------------+-------+--------+-------+--------+-------+--------+\n");
    printf("\n");
}

static void print_fan_values(char *sensor_name_to_print,
			     struct hw_sensor *hw_sensors_list,
			     int field_width, int precision_width
			    )
{
    struct hw_sensor *sensor_p;

    sensor_p = find_hw_sensor(sensor_name_to_print, hw_sensors_list);

    if (sensor_p) {
	if (isnan(sensor_p->read_val))
	    printf("%*s |", field_width, "N/A");
	else
	    printf("%s%*.*f%s |", sensor_p->alarm ? COLOR_RED : "",
		    field_width, precision_width, sensor_p->read_val,
		    sensor_p->alarm ? COLOR_OFF : "");
    } else {
	diot_error_add("BUG: sensor %s not on the sensors list!\n",
			sensor_name_to_print);
	printf("%s%*s%s |", COLOR_RED, field_width, "ERROR", COLOR_OFF);
    }
}

static void print_fantray_short(void)
{
    printf("Fantray readouts:\n");
    printf("-----------+-------------+\n");
    printf("Power rail | Voltage (V) |\n");
    printf("-----------+-------------+\n");
    printf("%10s |", "12V");
    print_fan_values("FANTRAY 12V Voltage", hw_sensors_monimod_list, 12, 2);
    printf("\n");
    printf("%10s |", "5V");
    print_fan_values("FANTRAY 5V Voltage", hw_sensors_monimod_list, 12, 2);
    printf("\n");
    printf("-----------+-------------+\n");

    printf("--------------------+-------+-------------+\n");
    printf("FAN                 |  RPM  | Current (A) |\n");
    printf("--------------------+-------+-------------+\n");
    printf("%-19s |", "FAN1 Slots S, P1-4");
    print_fan_values("FAN1 RPM Slots S+P1-4", hw_sensors_monimod_list, 6, 0);
    print_fan_values("FAN1 Current", hw_sensors_monimod_list, 12, 2);
    printf("\n");
    printf("%-19s |", "FAN2 RPM Slots P5-8");
    print_fan_values("FAN2 RPM Slots P5-8", hw_sensors_monimod_list, 6, 0);
    print_fan_values("FAN2 Current", hw_sensors_monimod_list, 12, 2);
    printf("\n");
    printf("%-19s |", "FAN3 RPM Power Supp");
    print_fan_values("FAN3 RPM Power Supp", hw_sensors_monimod_list, 6, 0);
    print_fan_values("FAN3 Current", hw_sensors_monimod_list, 12, 2);
    printf("\n");
    printf("--------------------+-------+-------------+\n");
    printf("\n");
}

int cmd_sensors(char *params)
{
    diot_error_clear();

    /* Check if PSU has been inserted or removed since last check */
    psu_added_or_removed(1);
    psu_added_or_removed(2);

    sensors_init(NULL);

    read_all_sensors();

    /* Print sensors */
    print_short_temp();
    print_irps_short();
    print_fantray_short();

    sensors_cleanup();
    diot_error_print();
    return 0;
}

int pts_sensors(void)
{
	int errors_temp = 0;
	int errors_vcp = 0;
	struct sensor_short *sens;
	struct hw_sensor *sensor_p;
	struct pts_sensor *pts_sensor_p;
	double val;
	struct short_vcp *svcp;

	sensors_init(NULL);
	read_all_sensors();

	sens = temp_sensors_list;
	printf("Test Temp sensors");
	for (; sens->short_name; sens++) {
		sensor_p = find_hw_sensor(sens->sensor_name, hw_sensors_temp_list);
		pts_sensor_p = find_pts_sensor(sens->short_name);
		if (sensor_p && pts_sensor_p) {
			if (!pts_sensor_inrange(sensor_p->read_val, pts_sensor_p)) {
				if (errors_temp++ == 0)
					printf("\rFAIL   Temp Sensors\n");
				printf("\t%s: ERROR (%.1f vs [%.1f;%.1f])\n", sensor_p->short_name,
						sensor_p->read_val, pts_sensor_p->allowed_min, pts_sensor_p->allowed_max);
			}
		} else {
			if (errors_temp++ == 0)
				printf("\rFAIL   Temp Sensors\n");
			printf("\t%s: ERROR sensor not found\n", pts_sensor_p->short_name);
		}
	}
	/* IRPS1 Temp */
	sens = temp_sensors_irps1_list;
	val = get_max_temp(temp_sensors_irps1_list, hw_sensors_temp_list, NULL);
	pts_sensor_p = find_pts_sensor(sens->short_name);
	if (!pts_sensor_inrange(val, pts_sensor_p)) {
		if (errors_temp++ == 0)
			printf("\rFAIL   Temp Sensors\n");
		printf("\t%s: ERROR (%.1f vs [%.1f;%.1f])\n", sens->short_name,
				val, pts_sensor_p->allowed_min, pts_sensor_p->allowed_max);
	}

	/* IRPS2 Temp */
	sens = temp_sensors_irps2_list;
	val = get_max_temp(temp_sensors_irps2_list, hw_sensors_temp_list, NULL);
	pts_sensor_p = find_pts_sensor(sens->short_name);
	if (!pts_sensor_inrange(val, pts_sensor_p)) {
		if (errors_temp++ == 0)
			printf("\rFAIL   Temp Sensors\n");
		printf("\t%s: ERROR (%.1f vs [%.1f;%.1f])\n", sens->short_name,
				val, pts_sensor_p->allowed_min, pts_sensor_p->allowed_max);
	}

	if (errors_temp == 0) {
		printf("\rOK     Temp sensors\n");
	}

	printf("Test Voltage/Current sensors");
	/* IRPS1 VCP */
	svcp = irps1_entries;
	for (; svcp->short_name; svcp++) {
		sensor_p = find_hw_sensor(svcp->voltage_sensor_name, hw_sensors_vcp_list);
		pts_sensor_p = find_pts_sensor(svcp->voltage_sensor_name);
		if (sensor_p && pts_sensor_p) {
			if (!pts_sensor_inrange(sensor_p->read_val, pts_sensor_p)) {
				if (errors_vcp++ == 0)
					printf("\rFAIL   Voltage/Current sensors");
				printf("\t%s: ERROR (%.2f vs [%.2f;%.2f])\n", svcp->voltage_sensor_name,
						sensor_p->read_val, pts_sensor_p->allowed_min,
						pts_sensor_p->allowed_max);
			}
		} else {
			if (errors_vcp++ == 0)
				printf("\rFAIL   Voltage/Current sensors");
			printf("\t%s: ERROR sensor not found\n", pts_sensor_p->short_name);
		}
	}
	/* IRPS1 Input Current */
	svcp = irps1_entries;
	sensor_p = find_hw_sensor(svcp->current_sensor_name, hw_sensors_vcp_list);
	pts_sensor_p = find_pts_sensor(svcp->current_sensor_name);
	if (sensor_p && pts_sensor_p) {
		if (!pts_sensor_inrange(sensor_p->read_val, pts_sensor_p)) {
			if (errors_vcp++ == 0)
				printf("\rFAIL   Voltage/Current sensors");
			printf("\t%s: ERROR (%.2f vs [%.2f;%.2f])\n", svcp->current_sensor_name,
					sensor_p->read_val, pts_sensor_p->allowed_min,
					pts_sensor_p->allowed_max);
		}
	} else {
		if (errors_vcp++ == 0)
			printf("\rFAIL   Voltage/Current sensors");
		printf("\t%s: ERROR sensor not found\n", pts_sensor_p->short_name);
	}

	/* IRPS2 VCP */
	svcp = irps2_entries;
	for (; svcp->short_name; svcp++) {
		sensor_p = find_hw_sensor(svcp->voltage_sensor_name, hw_sensors_vcp_list);
		pts_sensor_p = find_pts_sensor(svcp->voltage_sensor_name);
		if (sensor_p && pts_sensor_p) {
			if (!pts_sensor_inrange(sensor_p->read_val, pts_sensor_p)) {
				if (errors_vcp++ == 0)
					printf("\rFAIL   Voltage/Current sensors");
				printf("%s: ERROR (%.2f vs [%.2f;%.2f])\n", svcp->voltage_sensor_name,
						sensor_p->read_val, pts_sensor_p->allowed_min,
						pts_sensor_p->allowed_max);
			}
		} else {
			if (errors_vcp++ == 0)
				printf("\rFAIL   Voltage/Current sensors");
			printf("\t%s: ERROR sensor not found\n", pts_sensor_p->short_name);
		}
	}
	/* IRPS2 Input Current */
	svcp = irps2_entries;
	sensor_p = find_hw_sensor(svcp->current_sensor_name, hw_sensors_vcp_list);
	pts_sensor_p = find_pts_sensor(svcp->current_sensor_name);
	if (sensor_p && pts_sensor_p) {
		if (!pts_sensor_inrange(sensor_p->read_val, pts_sensor_p)) {
			if (errors_vcp++ == 0)
				printf("\rFAIL   Voltage/Current sensors");
			printf("%s: ERROR (%.2f vs [%.2f;%.2f])\n", svcp->current_sensor_name,
					sensor_p->read_val, pts_sensor_p->allowed_min,
					pts_sensor_p->allowed_max);
		}
	} else {
		if (errors_vcp++ == 0)
			printf("\rFAIL   Voltage/Current sensors");
		printf("\t%s: ERROR sensor not found\n", pts_sensor_p->short_name);
	}


	if (errors_vcp == 0) {
		printf("\rOK     Voltage/Current sensors\n");
	}

	sensors_cleanup();

	return errors_temp + errors_vcp;
}

static void print_expert_temp_print_sensor(struct sensor_short *temp_sensors_list,
					   struct hw_sensor *hw_sensors_list)
{
    int i;
    struct hw_sensor *hw_sens;

    for (i = 0; temp_sensors_list[i].sensor_name; i++) {
	hw_sens = find_hw_sensor(temp_sensors_list[i].sensor_name, hw_sensors_list);
	if (!hw_sens) {
	    diot_error_add("BUG: sensor %s not on the hw sensors list!\n",
			   temp_sensors_list[i].sensor_name);
	    continue;
	}

	printf("%-25s |", hw_sens->sensor_name);
	if(!isnan(hw_sens->read_val))
	    printf("%s%7.1f%s C |", ALARM_CHECK(hw_sens->alarm),
		   hw_sens->read_val, ALARM_OFF(hw_sens->alarm));
	else
	    printf("%9s |", "N/A");

	if(!isnan(hw_sens->max))
	    printf("%s%6.1f%s |", ALARM_CHECK(hw_sens->alarm),
		   hw_sens->max, ALARM_OFF(hw_sens->alarm));
	else
	    printf("%6s |", "");

	if(!isnan(hw_sens->crit_max))
	    printf("%s%6.1f%s |", ALARM_CHECK(hw_sens->alarm),
		   hw_sens->crit_max, ALARM_OFF(hw_sens->alarm));
	else
	    printf("%6s |", "");

	/* emerg */
	if(!isnan(hw_sens->emerg))
	    printf("%s%6.1f%s |", ALARM_CHECK(hw_sens->alarm),
		   hw_sens->emerg, ALARM_OFF(hw_sens->alarm));
	else
	    printf("%6s |", "");

	printf("\n");
    }
}

static void print_expert_temp(void)
{
    printf("Temp sensor               | read val |  high |  crit | emerg |\n");
    printf("--------------------------+----------+-------+-------+-------+\n");
    print_expert_temp_print_sensor(temp_sensors_list, hw_sensors_temp_list);
    print_expert_temp_print_sensor(temp_sensors_irps1_list, hw_sensors_temp_list);
    print_expert_temp_print_sensor(temp_sensors_irps2_list, hw_sensors_temp_list);
    print_expert_temp_print_sensor(temp_sensors_psu1_list, hw_sensors_temp_list);
    print_expert_temp_print_sensor(temp_sensors_psu2_list, hw_sensors_temp_list);
    print_expert_temp_print_sensor(temp_sensors_monimod_list, hw_sensors_temp_list);
    printf("--------------------------+----------+-------+-------+-------+\n");
}


char * double_or_blank(char * buff, int buff_size, double number)
{
    if (!isnan(number))
	snprintf(buff, buff_size, "%5.2f", number);
    else
	buff = "";
    return buff;
}

static void print_extend_vcp_vc(char *short_name, char *sensor_name,
					  struct hw_sensor *hw_sensors_list)
{
    struct hw_sensor *sensor_p;
    char buff[20];

    sensor_p = find_hw_sensor(sensor_name, hw_sensors_list);

    if (sensor_p) {
	printf("%*s", 5,
	       double_or_blank(buff, sizeof(buff), sensor_p->crit_min));
	printf("%*s", 7, double_or_blank(buff, sizeof(buff), sensor_p->min));
	printf("%s%*s%s", ALARM_CHECK(sensor_p->alarm), 9 + 2,
	       double_or_blank(buff, sizeof(buff), sensor_p->read_val),
	       ALARM_OFF(sensor_p->alarm));
	printf("%*s", 9 + 2,
	       double_or_blank(buff, sizeof(buff), sensor_p->max));
	printf("%*s", 7,
	       double_or_blank(buff, sizeof(buff), sensor_p->crit_max));

    } else {
	diot_error_add("BUG: sensor %s not on the sensors list! %s\n",
		       short_name, sensor_name);
	printf("%s%*s%*s%s", COLOR_RED, 23, "ERROR", 18, "", COLOR_OFF);
    }
}

static void print_extend_vcp_graph_vc(char *short_name, char *sensor_name,
				      struct hw_sensor *hw_sensors_list,
				      double crit_min, double crit_max,
				      int div_len
				     )
{
    struct hw_sensor *sensor_p;
    char buff[200];
    char *p = buff;
    double min = 0;
    double read_val;
    double max = 0;
    int len_min = 10 / div_len;
    int len_middle = 21 / div_len;
    int len_max = 10 / div_len;
    char mark_char = 'I';

    sensor_p = find_hw_sensor(sensor_name, hw_sensors_list);

    if (sensor_p && (!isnan(sensor_p->read_val))) {
	read_val = sensor_p->read_val;

	if (isnan(crit_min)) {
	    crit_min = 0;
	    if (!isnan(sensor_p->crit_min))
		crit_min = sensor_p->crit_min;
	}

	if (!isnan(sensor_p->min))
	    min = sensor_p->min;
	else {
	    min = crit_min;
	    len_middle += len_min;
	    len_min = 0;
	}

	if (isnan(crit_max)) {
	    crit_max = 24; /* Fallback */
	    if (!isnan(sensor_p->crit_max))
		crit_max = sensor_p->crit_max;
	}

	if (!isnan(sensor_p->max))
	    max = sensor_p->max;
	else {
	    max = crit_max;
	    len_middle += len_max;
	    len_max = 0;
	}
	memset(p, '=', len_min);
	p += len_min;
	memset(p, '-', len_middle);
	p += len_middle;
	memset(p, '=', len_max);
	p += len_max;
	*p = 0;


	if (read_val < crit_min)
	    buff[0] = mark_char;
	else if (read_val < min)
	    buff[(int)(len_min * (read_val - crit_min)/(min - crit_min))] = mark_char;
	else if (read_val < max){
	    buff[(int)(len_min + len_middle * (read_val - min)/(max - min))] = mark_char;
	}
	else if (read_val < crit_max){
	    buff[(int)(len_min + len_middle + len_max * (read_val - max)/(crit_max-max))] = mark_char;
	} else
	    buff[strlen(buff) - 1] = mark_char;
	printf("%s", buff);
    } else {
	printf("%s%*s%*s%s", COLOR_RED, 23, "ERROR", 18, "", COLOR_OFF);
    }
}

static void print_expert_vcp(struct short_vcp *vcp_list,
				    struct hw_sensor *hw_sensors_list)
{
    struct short_vcp *vcp_to_print = vcp_list;
    double max_current;

    for (; vcp_to_print->short_name; vcp_to_print++) {
	printf("%-14s | ", vcp_to_print->short_name);

	print_extend_vcp_vc(vcp_to_print->short_name,
				      vcp_to_print->voltage_sensor_name,
				      hw_sensors_list);
	printf(" | ");
	print_extend_vcp_vc(vcp_to_print->short_name,
				      vcp_to_print->current_sensor_name,
				      hw_sensors_list);
	printf(" | ");
	print_extend_vcp_vc(vcp_to_print->short_name,
				      vcp_to_print->power_sensor_name,
				      hw_sensors_list);
	printf(" |\n");

	max_current = NAN;
	if (!strcmp(vcp_to_print->short_name, "Input"))
	    max_current = 4;
	printf("%-14s | ", "");
	print_extend_vcp_graph_vc(vcp_to_print->short_name,
				      vcp_to_print->voltage_sensor_name,
				      hw_sensors_list, NAN, NAN, 1);
	printf(" | ");
	print_extend_vcp_graph_vc(vcp_to_print->short_name,
				      vcp_to_print->current_sensor_name,
				      hw_sensors_list, NAN, max_current, 1);
	printf(" | ");
	print_extend_vcp_graph_vc(vcp_to_print->short_name,
				      vcp_to_print->power_sensor_name,
				      hw_sensors_list, NAN, NAN, 1);
	printf(" |\n");

	printf("---------------+-------------------------------------------+-------------------------------------------+-------------------------------------------+\n");
    }
}

static void print_expert_irps(void)
{
    printf("---------------+-------------------------------------------+-------------------------------------------+-------------------------------------------+\n");
    printf("IRPS1          |                 Voltage (V)               |                Current (A)                |                Power (W)                  |\n");
    printf("IRPS1          | min_crit min       read      max max_crit | min_crit min       read      max max_crit |                                           |\n");
    printf("---------------+-------------------------------------------+-------------------------------------------+-------------------------------------------+\n");
    print_expert_vcp(irps1_entries, hw_sensors_vcp_list);
    printf("IRPS2          |                 Voltage (V)               |                Current (A)                |                Power (W)                  |\n");
    printf("---------------+-------------------------------------------+-------------------------------------------+-------------------------------------------+\n");
    print_expert_vcp(irps2_entries, hw_sensors_vcp_list);
    printf("PSU            |                 Voltage (V)               |                Current (A)                |                Power (W)                  |\n");
    printf("---------------+-------------------------------------------+-------------------------------------------+-------------------------------------------+\n");

    print_expert_vcp(psu1_entries, hw_sensors_vcp_list);
    print_expert_vcp(psu2_entries, hw_sensors_vcp_list);

    printf("\n");
}

int cmd_sensors_expert(char *params)
{
    diot_error_clear();

    /* Check if PSU has been inserted or removed since last check */
    psu_added_or_removed(1);
    psu_added_or_removed(2);

    sensors_init(NULL);

    read_all_sensors();

    /* Display sensors */
    print_expert_temp();
    printf("\n");
    print_expert_irps();
    printf("\n");
    print_fantray_short();
    sensors_cleanup();
    diot_error_print();

    return 1;
}

int cmd_sensors_refresh(char *params)
{
    time_t cur_sec, last_update_sec = 0;

    term_clear();
    term_init(1);
    setvbuf(stdout, NULL, _IOFBF, 8192);

    for(;;) {
	if (term_poll(10)) {
	    int c = term_get();

	    switch (c) {
	    case 'q':
		    goto quit;
	    /* Placeholder for future keys */
	    }
	}

	cur_sec = get_monotonic_sec();
	if (cur_sec > last_update_sec) {
	    last_update_sec = cur_sec;
	    /* Set cursor to position 0,0 (top left corner */
	    printf("\033[%d;%df", 0, 0);
	    term_clear_to_end();

	    cmd_sensors(NULL);

	    fflush(stdout);
	}

	if (ferror(stdout))
	exit(1);
    }

quit:
    term_restore();
    setlinebuf(stdout);
    printf("\n");
    return 1;
}
