/*
	Part of the BMS55 firmware.
	(c) Perttu Ahola
*/

#define F_CPU 8000000L
#include <util/delay.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include <util/atomic.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "../common/nodebus.h"

#define VERSION_STRING "0.0.3"
#define IDENTIFY_STRING "bms55-" VERSION_STRING

/*
TODOS

FUSES
	hfuse=0xc4: WDT always on, EEPROM preserved, 4.3V brown-out
	lfuse=0xe2: CKDIV8 unprogrammed, Internal oscillator

GENERAL
	This is the main MCU of the Master board.

RANDOM
	- Timer 0 is a temporary timer
	- Timer 1 is used by the main loop
	- No information of every cell is kept in memory, because of limited
	  resources.

DESIGN PRINCIPLES
	The protocols used have been designed on top of the idea, that:	
	a) Master has to be able to determine if any of the nodes have a problem
	b) The UI device has to be able to determine if the master has a problem
	-> The user is able to determine if there is a problem, by checking that:
		a) The UI device is working
		b) The UI device tells that the Master is working and the Master
		   is saying all the nodes are fine.
	   - If a) and b) are true, everything is working.
	   - If a) is false, something else can be broken too.
	   - If a) is true and b) is false, the case is obvious.

	In case of error, everything defaults to a state that is safe in as many
	different situations as possible.
	
	Additionally, correct error reporting is important.
	- All information that is useful for a connected device to easily
	  determine if a device is misbehaving has to be available.
	
	Coding principles:
	- Do only stuff that has to be done.
	- If stuff has to be done, do it only in one place.
	- Do not do same stuff in many places.
	- Use ugly macro hacks to remove duplicate code and for smaller resource
	  consumption.
	- Use descriptive and long variable names.
	- If code is not obvious, use comments.
	- If non-obvious code is hard to comment, change code.

RS232 PROTOCOL:
	Baud rate is 9600 bps.

	TODO: This is largely outdated.
	TODO: statusnodes

	- Line format from BMS to Client:
		"<title>:<valuename>=<value>,[...][,]\r\n"
	- Order of values is not constant. There can be an ending comma, or not.
	- Titles and valuenames are in lowercase.

	- Line format from Client to BMS:
		"<command> [parameter] [...]\r\n"
	
	- The BMS doesn't echo back characters.
	- For convenience, the backspace character (127) is supported for
	  easier debugging on a serial terminal.

	Clients should allow and ignore unknown titles and valuenames. It might be
	wise to report a possible version mismatch somehow to the user, though.

	When a line (ending in "\r\n") is sent to the BMS, the client has to
	wait for the BMS to send out a '$', after which the client can send an
	another line. '$' is always sent on its own line.
	
	Example communication (C = Client->BMS, B = BMS->Client):

	Client can send an empty command "\r\n" to see if the BMS is awake.
	If it is, it will reply with
		B: "protocol:commands=version status set nodes\r\n"
	or similar, followed by a '$'
	
	- The BMS replies fairly slowly. Timeout should generally be a few
	  seconds for a 12-cell system.

	- Asking node info:
		- This command will be awfully slow with many cells and 
		C: "nodes\r\n"
		B: "nodes:count=12\r\n"
		B: "node:n=1,status=commfail ,\r\n"
		B: "node:n=2,status=text_hl,volt=3527,temp=20,shunt=0,text=Testing,\r\n"
		B: "node:n=3,status=error_nop commfail,\r\n"
		B: ...

		Possible values for "nodes:"
		- count=integer // The number of nodes

		Possible values for "node:"
		- n=integer // Node number (1 - 254)
		- status (can be many)
			=commfail // Fails to reply or replies random things
			=cellfail // Cell is getting damaged because of failure
			=elecfail // Electronics are getting damaged because of failure
			=error_nop // There is a non-fatal error. No operation allowed.
			=text_hl // The status text should be hilighted to the user
		- volt=integer // Node voltage (millivolts)
		- temp=integer // Node temperature (degrees celsius)
		- shunt=integer // Shunt usage, 0-255, where 127 is maximum continuous usage
		- text=string // Status text, can include spaces, should not include a '$',
		                 maximum length is 16.
	
	- Handling settings:
		- The client can ask for all the settings:
		C: "set\r\n"
		B: "set:cell_count=12\r\n"
		B: ...

		- The client can ask for a single setting:
		C: "set cell_voltage_max_fatal\r\n"
		B: "set:cell_voltage_max_fatal=4400\r\n"

		- The client can change a setting:
		C: "set cell_voltage_min 2800\r\n"
		B: "set:cell_voltage_min=2800\r\n"

		- The values of the settings are integers.
		- The settings are saved in EEPROM, so they should not be changed
		  very often. The EEPROM lasts for 100 000 read/write cycles, so a
		  few times a day doesn't hurt.

		// TODO: list of settings
		- Here's a crude list of settings copied from the code:
		SETTING_VARIABLE(uint8_t, cell_count, 12);
		SETTING_VARIABLE(uint16_t, cell_voltage_min_fatal, 2000);
		SETTING_VARIABLE(uint16_t, cell_voltage_max_fatal, 4400);
		SETTING_VARIABLE(uint16_t, cell_voltage_min, 3000);
		SETTING_VARIABLE(uint16_t, cell_voltage_max, 4200);
		SETTING_VARIABLE(int8_t, cell_temperature_min_fatal, -30);
		SETTING_VARIABLE(int8_t, cell_temperature_max_fatal, 50);
		SETTING_VARIABLE(uint8_t, debug_jam, 0);
		SETTING_VARIABLE(uint16_t, charge_current_max, 1000);

	
	- Asking status:
		- The client can ask for status:
		C: "status\r\n"
		B: status:fatal_error=0
		B: status:uptime=1278
		B: status:voltage_psu=18177
		B: ...

		- Status variables are integers and strings.

		// TODO: list of status variables
		- Again, a crude listing from the code:
		VAR_TRANSMIT_UINT8("fatal_error", g_fatal_error);
		VAR_TRANSMIT_INT32("uptime", g_uptime_seconds);
		VAR_TRANSMIT_INT32("voltage_psu", g_voltage_psu);
		VAR_TRANSMIT_INT32("voltage_battery", g_voltage_battery);
		VAR_TRANSMIT_INT32("node_voltage_sum", g_node_voltage_sum);
		VAR_TRANSMIT_INT32("charge_current", g_charge_current);
		VAR_TRANSMIT_UINT8("settings_saved", g_settings_changed == -1 ? 1 : 0);
	
	- Asking version:
		- The client can ask for the version of the Master:
		C: "version\r\n"
		B: "version:bms55-0.0.1\r\n"
	
	- Calibrating temperature:
		- The client can calibrate the temperature sensors of the system.
		- E.g. calibrate to 20 degrees celsius
		C: "temp_calibrate 20\r\n"
		- The operation is carried out asynchronously with updating the
		  nodes, and will finish in a few seconds for a 12-cell system.
		- Client can check if calibration was successful by checking the
		  temperatures reported by the nodes after two "nodes" queries.
	
	Messages sent by the Master:
	
	- info: These lines should probably be ignored by most clients
		B: "info: bms55 (c) Perttu Ahola <celeron55@gmail.com>\r\n"
		- These lines don't follow the valuename=value rule.
	
	- protocol: Sent when the Master doesn't understand what the client is
	  sending. The client should try again whatever it did - if it fails
	  again, it should report a communication error to the user.
		C: "order kebab\r\n"
		B: "protocol:commands=version status set nodes\r\n"
		C: "order kebab\r\n"
		B: "protocol:commands=version status set nodes\r\n"
		C: "fuck it.\r\n"
		B: "protocol:commands=version status set nodes\r\n"
	
	- bootup: At boot-up, the BMS reports a possible reset source:
		- Any of these:
		B: "bootup:reset=watchdog\r\n"
		B: "bootup:reset=brown-out\r\n"
		B: "bootup:reset=external\r\n"
		B: "bootup:reset=none\r\n"

		- A watchdog reset should be considered as a fairly serious error.
		  It can occur repeatedly in case of a bug in the software.

		- A brown-out reset means low voltage. Coming out of nowhere, it means
		  unstable voltage. It usually occurs at normal boot-up.

		- An external reset can occur when the chip is programmed, for example.
		  Should not occur in normal usage.
	
	An example session:
		// Let's assume the client has just booted up and is connected to
		// the BMS.
		// Client sends an empty command to see if BMS is alive
		C: "\r\n"
		B: "protocol:commands=version status set nodes temp_calibrate\r\n"
		B: "$"
		// OK, BMS takes commands. Client might check version for maximum
		// compatibility.
		C: "version\r\n"
		B: "version:bms55-0.0.1\r\n"
		B: "$"
		// Gather some data for showing to the user
		C: "status\r\n"
		B: "status:fatal_error=1\r\n"
		B: "status:uptime=402054\r\n"
		B: "status:voltage_psu=47660\r\n"
		B: "status:voltage_battery=39600\r\n"
		B: "status:voltage_node_sum=0\r\n"
		B: "status:current_charger=0\r\n"
		B: "status:settings_saved=1\r\n"
		B: "$"
		// Looks like there is a serious problem...
		// Ask for info about the nodes
		C: "nodes\r\n"
		B: "nodes:count=12\r\n"
		B: "node:n=1,status=commfail ,\r\n"
		B: "node:n=2,status=commfail ,\r\n"
		B: "node:n=3,status=commfail ,\r\n"
		B: "node:n=4,status=commfail ,\r\n"
		B: "node:n=5,status=commfail ,\r\n"
		B: "node:n=6,status=commfail ,\r\n"
		B: "node:n=7,status=commfail ,\r\n"
		B: "node:n=8,status=commfail ,\r\n"
		B: "node:n=9,status=commfail ,\r\n"
		B: "node:n=10,status=commfail ,\r\n"
		B: "node:n=11,status=commfail ,\r\n"
		B: "node:n=12,status=commfail ,\r\n"
		B: "$"
		// Now this looks quite fatal. No nodes at all are connected!
		// Assume that the user goes to a "settings" menu.
		// Client probably caches the settings, but it's a good time to
		// update those.
		C: "set\r\n"
		B: "set:cell_count=12\r\n"
		B: "set:cell_voltage_min_fatal=2000\r\n"
		B: "set:cell_voltage_max_fatal=4400\r\n"
		B: "set:cell_voltage_min=3000\r\n"
		B: "set:cell_voltage_max=4200\r\n"
		B: "set:cell_temperature_min_fatal=-30\r\n"
		B: "set:cell_temperature_max_fatal=50\r\n"
		B: "set:debug_jam=0\r\n"
		B: "set:charge_current_max=500\r\n"
		B: "$"
		// The user changes a setting.
		C: "set cell_count 12\r\n"
		B: "set:cell_count=12\r\n"
		B: "$"

*/

/*
	Helper macros and functions
*/
#define TIMER_US(TIMER_CLOCK, TIME_US) ((TIMER_CLOCK) * (TIME_US) / 1000000)

/*
	A quick one, returns 0 or 1
*/
/*uint8_t strncmp_P(const char *mem, const char *progmem, uint8_t len)
{
	uint8_t i;
	for(i=0; i<len; i++){
		if(*mem != pgm_read_byte(progmem))
			return 1;
		if(*mem == 0 || pgm_read_byte(progmem) == 0)
			break;
		mem++;
		progmem++;
	}
	return 0;
}*/

#define d_sendtext(x) usart_transmit_text_P(PSTR(x))
#define d_sendvar(x, name){\
	char buf[12];\
	ltoa(x, buf, 10);\
	d_sendtext("[" name "=");\
	usart_transmit_text(buf);\
	d_sendtext("]");\
}

#define RAISE(x, atleast){ if(x < atleast) x = atleast; }

void usart_transmit_text(const char *p);
void usart_transmit_text_P(const char *p);
//static uint16_t adc_get(uint8_t channel);

/*#define debugprint(bufsize, x, ...){\
	char buf[bufsize];\
	snprintf(buf, bufsize, x, __VA_ARGS__);\
	usart_transmit_text(buf);\
}*/

enum settingtype
{
	VARTYPE_uint8_t=1,
	VARTYPE_int8_t,
	VARTYPE_uint16_t,
	VARTYPE_uint32_t,
};

struct setting
{
	enum settingtype type;
	const char *name;
	void *ptr;
	void *ee;
	/*int32_t min;
	int32_t max;*/
};

#define SETTING_ENTRY(ee_address, type, name){\
	VARTYPE_ ## type,\
	name ## _name,\
	(void*)&g_ ## name,\
	(void*)ee_address\
}

#define SETTING_VARIABLE(type, setting, value)\
	volatile type g_ ## setting = value;\
	const char setting ## _name[] PROGMEM = #setting;


SETTING_VARIABLE(uint8_t, cell_count, 12);
SETTING_VARIABLE(uint16_t, cell_voltage_min_fatal, 2000);
SETTING_VARIABLE(uint16_t, cell_voltage_max_fatal, 4300);
SETTING_VARIABLE(uint16_t, cell_voltage_min, 2500);
SETTING_VARIABLE(uint16_t, cell_voltage_max, 3600);
SETTING_VARIABLE(int8_t, cell_temperature_min_fatal, -30);
SETTING_VARIABLE(int8_t, cell_temperature_max_fatal, 50);
SETTING_VARIABLE(uint8_t, debug_jam, 0);
SETTING_VARIABLE(uint16_t, charge_current_max, 1000);
SETTING_VARIABLE(uint8_t, charge_on_error, 0);

/*
	The addresses of these should be preserved through changes.

	debug_jam is for testing that settings won't be saved if the system
	jams because of a setting change.
*/
struct setting settings[] PROGMEM =
{
	SETTING_ENTRY(0,  uint8_t, cell_count),
	SETTING_ENTRY(1,  uint16_t, cell_voltage_min_fatal),
	SETTING_ENTRY(3,  uint16_t, cell_voltage_max_fatal),
	SETTING_ENTRY(5,  uint16_t, cell_voltage_min),
	SETTING_ENTRY(7,  uint16_t, cell_voltage_max),
	SETTING_ENTRY(9,  int8_t, cell_temperature_min_fatal),
	SETTING_ENTRY(10, int8_t, cell_temperature_max_fatal),
	SETTING_ENTRY(11, uint8_t, debug_jam),
	SETTING_ENTRY(12, uint16_t, charge_current_max),
	SETTING_ENTRY(14, uint8_t, charge_on_error),
	{0},
};

void eeprom_load_settings(void)
{
	struct setting *v = settings;
	for(; pgm_read_byte(&v->type) != 0; v++){
		switch(pgm_read_byte(&v->type)){
			case VARTYPE_uint8_t: {
				uint8_t temp = eeprom_read_byte((void*)pgm_read_word(&v->ee));
				if(temp != 0xff)
					*((uint8_t*)pgm_read_word(&v->ptr)) = temp;
				break;
			}
			case VARTYPE_int8_t: {
				uint8_t temp = eeprom_read_byte((void*)pgm_read_word(&v->ee));
				if(temp != 0xff)
					*((int8_t*)pgm_read_word(&v->ptr)) = temp - 128;
				break;
			}
			case VARTYPE_uint16_t: {
				uint16_t temp = eeprom_read_word((void*)pgm_read_word(&v->ee));
				if(temp != 0xffff)
					*((uint16_t*)pgm_read_word(&v->ptr)) = temp;
				break;
			}
			case VARTYPE_uint32_t: {
				uint32_t temp = eeprom_read_dword((void*)pgm_read_word(&v->ee));
				if(temp != 0xffffffff)
					*((uint32_t*)pgm_read_word(&v->ptr)) = temp;
				break;
			}
		}
	}
}

void eeprom_write_settings(void)
{
	struct setting *v = settings;
	for(; pgm_read_byte(&v->type) != 0; v++){
		switch(pgm_read_byte(&v->type)){
			case VARTYPE_uint8_t: {
				uint8_t temp = *((uint8_t*)pgm_read_word(&v->ptr));
				if(temp != 0xff)
					eeprom_write_byte((void*)pgm_read_word(&v->ee), temp);
				break;
			}
			case VARTYPE_int8_t: {
				int8_t temp = *((int8_t*)pgm_read_word(&v->ptr));
				temp += 128;
				if(temp != 0xff)
					eeprom_write_byte((void*)pgm_read_word(&v->ee), temp);
				break;
			}
			case VARTYPE_uint16_t: {
				uint16_t temp = *((uint16_t*)pgm_read_word(&v->ptr));
				if(temp != 0xff)
					eeprom_write_word((void*)pgm_read_word(&v->ee), temp);
				break;
			}
			case VARTYPE_uint32_t: {
				uint32_t temp = *((uint32_t*)pgm_read_word(&v->ptr));
				if(temp != 0xff)
					eeprom_write_dword((void*)pgm_read_word(&v->ee), temp);
				break;
			}
		}
	}
}

//#define RS232_BAUD 38400
#define RS232_BAUD 9600
#define CHARGE_CURRENT_MAX 13000
#define CHARGE_CURRENT_STOP 100

#define LED_DDR DDRE
#define LED_PORT PORTE
#define LED_BIT 2

#define E_LED_DDR DDRD
#define E_LED_PORT PORTD
#define E_LED_BIT 5

// DRIVER_L is controlled by PSCOUT00
// DRIVER_H is controlled by PSCOUT01
#define DRIVER_L_DDR DDRD
#define DRIVER_L_BIT 0
#define DRIVER_H_DDR DDRB
#define DRIVER_H_BIT 7

#define RELAY_CTRL_DDR DDRB
#define RELAY_CTRL_PORT PORTB
#define RELAY_CTRL_BIT 1

#define NODE_TX_DDR DDRC
#define NODE_TX_PORT PORTC
#define NODE_TX_BIT 3

#define NODE_RX_DDR DDRB
#define NODE_RX_PORT PORTB
#define NODE_RX_PORT_IN PINB
#define NODE_RX_BIT 2

// PSC 0 Prescaler select:
// PPRE01 PPRE00 Description
// 0      0      No divider
// 0      1      Divide PCB input clock by 4
// 1      0      Divide by 32 (at90pwm2/3: divide by 16)
// 1      1      Divide by 256 (at90pwm2/3: divide by 64)
// PBFM0: Balance Flank Width Modulation (well, FWM doesn't work but anyway)
#define PCTL0_SETTINGS ((0<<PPRE01) | (0<<PPRE00) | (1<<PBFM0))

#define PSC_SELECTED_PCLKSEL (1<<PCLKSEL0) // Fast clock, 64MHz PLL
// PSC 0 Center Aligned Mode + clock select + output active low
//#define PCNF_SETTING ((1<<PMODE01)|(1<<PMODE00)|(1<<POP0)|PSC_SELECTED_PCLKSEL)
// One Ramp Mode + clock select + output active high
#define PCNF_SETTING ((0<<PMODE01)|(0<<PMODE00)|(1<<POP0)|(PSC_SELECTED_PCLKSEL))
//#define PCNF_SETTING ((PSC_SELECTED_PCLKSEL))

#define PSC_CLOCK 64000000L
//#define PWM_FREQ 350000L
#define PWM_FREQ 250000L
//#define PWM_FREQ 500000L
#define PWM_CYCLETIME (PSC_CLOCK/PWM_FREQ)
//#define PWM_DEADTIME (PSC_CLOCK/10000000) // 0.1us (10 000 000th a second)
#define PWM_DEADTIME (PSC_CLOCK/5000000) // 0.2us (5 000 000th a second)
#define PWM_MINIMUM_LOWTIME (PSC_CLOCK/5000000) // 0.2us
//#define PWM_MINIMUM_LOWTIME (PWM_CYCLETIME/10)
#define PSC_SA (PWM_DEADTIME)
#define PSC_RA (PWM_DEADTIME + PWM_MINIMUM_LOWTIME)
#define PSC_RB (PWM_CYCLETIME - 1)
#define PSC_MAX (PSC_RB)
#define PWM_MAX (PSC_RB - PWM_DEADTIME*2 - PWM_MINIMUM_LOWTIME)
/*#define PSC_MAX (PSC_RB<<4)
#define PWM_MAX ((PSC_RB - PWM_DEADTIME*2 - PWM_MINIMUM_LOWTIME)<<4)*/

#define TIMER1_CLOCK (F_CPU/1024)

// External VREF, No left-adjust
#define ADMUX_SETTINGS ((0<<REFS1)|(0<<REFS0)|(0<<ADLAR))
// clock=F_CPU/8, enable
//#define ADCSRA_SETTINGS ((0<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADEN))
// clock=F_CPU/64, enable
//#define ADCSRA_SETTINGS ((1<<ADPS2)|(1<<ADPS1)|(0<<ADPS0)|(1<<ADEN))
/*
	Use lower speed so that the interrupt doesn't slow down the code
	too much.
*/
// clock=F_CPU/128, enable, enable interrupt
#define ADCSRA_SETTINGS ((1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADEN)|(1<<ADIE))

#define ADC_CH_BATTERY 7
#define ADC_CH_PSU 6
//#define ADC_CH_CHARGE_CURRENT 10 // max4081
#define ADC_CH_CHARGE_SHUNT 11 // amp0
#define ADC_CH_SHUNT 12 // amp1

//#define AREF_V 2520.
#define AREF_V (4950/2.)

#define VOLTAGE_SCALE ((uint32_t)((680.*2/47.+1) * AREF_V / 1024))
/*#define CHARGE_CURRENT_SCALE ((uint32_t)((2500./60/(CHARGE_RESISTOR)/1024)))*/
/*#define CHARGE_RESISTOR 0.75e-3
#define CHARGE_CURRENT_SCALE ((uint32_t)((5000./40/(CHARGE_RESISTOR)/1024)))*/
#define CHARGE_RESISTOR 0.1
#define CHARGE_CURRENT_SCALE ((uint32_t)((5000./5/(CHARGE_RESISTOR)/1024)))

#define AMP0_SETTING_GAIN ((0<<AMP0G1)|(0<<AMP0G0)) // 5
//#define AMP0_SETTING_GAIN ((0<<AMP0G1)|(1<<AMP0G0)) // 10
//#define AMP0_SETTING_GAIN ((1<<AMP0G1)|(0<<AMP0G0)) // 20
//#define AMP0_SETTING_GAIN ((1<<AMP0G1)|(1<<AMP0G0)) // 40

#define AMP1_SETTING_GAIN ((1<<AMP0G1)|(1<<AMP0G0)) // 40

volatile int32_t g_voltage_psu = 0;
volatile int32_t g_voltage_battery = 0;
volatile int32_t g_node_voltage_sum = 0;
volatile int16_t g_charge_current = 0;
volatile int32_t g_mah_charger = 0;
volatile uint8_t g_pack_voltage_is_node_voltage_sum = 0;
volatile int32_t g_uptime_seconds = 0;
volatile uint8_t g_errorlevel = 0;
// Do not use ":" in this
volatile PGM_P g_statustext = "";
volatile uint8_t g_comm_error_count = 0;

// Charger is controlled according to these
volatile uint8_t g_charging = 0;
volatile int16_t g_wanted_charge_current = 0;

/*#define CELL_COUNT_MAX 20
volatile int32_t g_cell_capacities[CELL_COUNT_MAX] = {0};*/

/*
	If a shell command sets this, the re-enabling of input is postponed.
	The RS232 RX is re-enabled by calling:
		usart_rx_enable();
		prompt();
*/
volatile uint8_t g_command_continues = 0;

/*
	When set, information of all nodes will be relayed on RS232
	at next update
*/
volatile uint8_t g_node_info_requested = 0;

/*
	When set, the temperature calibration command of the nodes will
	be called at next update.
*/
volatile uint8_t g_temperature_calibration_requested = 0;
volatile int8_t g_temperature_calibration_value;

volatile uint8_t g_reset_shunt_mah_requested = 0;

/*
	When a setting is changed, this is set to
	SETTINGS_CHANGED_COUNT

	Then, the main loop counts it down and saves settings
	after a while of successfull operation.
*/
volatile int8_t g_settings_changed = -1;
#define SETTINGS_CHANGED_COUNT 5

//volatile uint8_t g_node_info_requested_debug = 0;


inline void timer0_div(uint16_t divider)
{
	switch(divider){
	case 1:
		TCCR0B = (1<<CS00);
		break;
	case 8:
		TCCR0B = (1<<CS01)|(0<<CS00);
		break;
	case 64:
		TCCR0B = (1<<CS01)|(1<<CS00);
		break;
	case 256:
		TCCR0B = (1<<CS02)|(0<<CS01)|(0<<CS00);
		break;
	case 1024:
		TCCR0B = (1<<CS02)|(0<<CS01)|(1<<CS00);
		break;
	default:
		usart_transmit_text_P(PSTR("error: invalid timer0_div\r\n"));
	}
}

static void io_init(void)
{
	LED_DDR |= (1<<LED_BIT);
	E_LED_DDR |= (1<<E_LED_BIT);
	DRIVER_L_DDR |= (1<<DRIVER_L_BIT);
	DRIVER_H_DDR |= (1<<DRIVER_H_BIT);
	RELAY_CTRL_DDR |= (1<<RELAY_CTRL_BIT);
	NODE_TX_DDR |= (1<<NODE_TX_BIT);
	NODE_TX_PORT |= (1<<NODE_TX_BIT);
	NODE_RX_DDR &= ~(1<<NODE_RX_BIT);
	NODE_RX_PORT |= (1<<NODE_RX_BIT); // pull-up
	// Pull-up floating pins
	PORTB |= (1<<2) | (1<<0);
	PORTC |= (1<<7) | (1<<1) | (1<<0);
	PORTD |= (1<<7) | (1<<6) | (1<<1);
	PORTE |= (1<<1);
}

static void psc_init(void)
{
	/*
	Driver lowside input is connected to PSCOUT00
	Driver highside input is connected to PSCOUT01
	*/

	// Enable PLL Oscillator at 64MHz
	PLLCSR = (1<<PLLF) | (1<<PLLE);
	// Wait for it to start
	while(!(PLLCSR & (1<<PLOCK)));

	/*
		PSC0 Part B Output Enable (PSC0A = PSCOUT01)
		PSC0 Part A Output Enable (PSC0A = PSCOUT00)
	*/
	PSOC0 = (1<<POEN0B) | (1<<POEN0A);
	
	PCNF0 = PCNF_SETTING;
	
	// OCRnSA - Controls start of on-time 0
	OCR0SA = PSC_SA;
	// OCRnRA - Controls end of on-time 0
	OCR0RA = PSC_RA;
	// OCRnSB - Controls start of on-time 1
	OCR0SB = PSC_RB+1;
	// OCRnRB - Controls end of on-time 1 and cycle time
	OCR0RB = PSC_RB;
}

static void psc_lock(void)
{
	PCNF0 = (1<<PLOCK0) | PCNF_SETTING;
}

static void psc_unlock(void)
{
	PCNF0 = PCNF_SETTING;
}

static void psc_start(void)
{
	PCTL0 = PCTL0_SETTINGS | (1<<PRUN0);
}

static void psc_stop(void)
{
	PCTL0 = PCTL0_SETTINGS | (0<<PRUN0);
}

/*
	PWM: 0 - PSC_MAX
	Remember to leave some headroom for bootstrapping.
*/
//volatile uint16_t g_pwm_value = 0;

static void psc_set(uint16_t pwm)
{
	if(pwm > PWM_MAX)
		pwm = PWM_MAX;
		
	psc_lock();
	
	OCR0SB = PSC_RB - pwm; // Start of on-time 1

	// Enhanced precision
	/*uint16_t a = pwm & 0x000f;
	OCR0SB = PSC_RB - ((pwm >>4));
	if(a >= 8){
		OCR0RB = PSC_RB + 1;
	}
	else{
		OCR0RB = PSC_RB;
	}*/
	//OCR0RB = PSC_RB | ((pwm & 0x000f) << 12);

	//g_pwm_value = pwm;

	psc_unlock();
}

inline uint16_t psc_get(void)
{
	//return g_pwm_value;
	return PSC_RB - OCR0SB;
}

static void timer1_init(void)
{
	// clk_io/64
	//TCCR1B = (0<<CS12) | (1<<CS11) | (1<<CS10);
	
	// clk_io/1024
	TCCR1B = (1<<CS12) | (0<<CS11) | (1<<CS10);
}

static void adc_init(void)
{
	ADMUX = ADMUX_SETTINGS;
	/*
		Start converting right away; interrupt is enabled in
		ADCSRA_SETTINGS.
	*/
	ADCSRA = ADCSRA_SETTINGS | (1<<ADSC);

	// Amplifier 0 enable, gain setting, auto-sync on adc clock / 8
	AMP0CSR = (1<<AMP0EN)|AMP0_SETTING_GAIN|(0<<AMP0TS1)|(0<<AMP0TS0);
	// Amplifier 1 enable, gain setting, auto-sync on adc clock / 8
	AMP1CSR = (1<<AMP1EN)|AMP1_SETTING_GAIN|(0<<AMP1TS1)|(0<<AMP1TS0);
}

/*
	NODEBUS FUNCTIONS
*/

#define NODEBAUD_TIMER0_BYTELEN (F_CPU/8/NODEBUS_BAUD)

#define NODEBUS_DATA_RXFAIL 0xffff

volatile uint16_t g_nodebus_txdata;

// Actually the largest size used is 2, when storing an ACK and a reply byte.
#define NODEBUS_RXBUF_SIZE 3
volatile uint16_t g_nodebus_raw_rxbuf[NODEBUS_RXBUF_SIZE];
volatile uint8_t g_nodebus_raw_rxbuf_len = 0;

inline void nodebus_raw_rxbuf_insert(uint16_t d)
{
	uint8_t l = g_nodebus_raw_rxbuf_len;
	if(l >= NODEBUS_RXBUF_SIZE)
		return;
	g_nodebus_raw_rxbuf[l] = d;
	g_nodebus_raw_rxbuf_len = l + 1;
}

volatile int8_t g_nodebus_counter; // -1 = start bit, 0-8 = data, 9 = stop

enum {
	NODEBUS_READY,
	NODEBUS_TXBUSY,
	//NODEBUS_TXSTOP, // delay for stop bits
	NODEBUS_RXBUSY,
	NODEBUS_RXWAIT, // wait for start bit
} volatile g_timer0_state = NODEBUS_READY;

/*
	When this is set to >0, a raw_receive is started immediately
	after a successful raw_transmit or raw_receive, and this is
	decremented.
*/
volatile uint8_t g_nodebus_schedule_raw_receives = 0;

inline uint8_t node_raw_receive_start(void);

inline void nodebus_run_schedule(void)
{
	if(g_nodebus_schedule_raw_receives > 0){
		g_nodebus_schedule_raw_receives--;
		if(node_raw_receive_start()){
			// Cancel all scheduled receives if receive failed
			g_nodebus_schedule_raw_receives = 0;
		}
	}
}

/*
	This is the only interrupt that is allowed to not enable interrupts
	while it is executing.
*/
ISR(TIMER0_OVF_vect)
{
	//DEBUG
	/*g_timer0_state = NODEBUS_READY;
	return;*/
	TCNT0 -= NODEBAUD_TIMER0_BYTELEN;
	if(g_timer0_state == NODEBUS_TXBUSY){
		int8_t counter = g_nodebus_counter;
		uint16_t bit;
		if(counter == -1)
			bit = 0;
		else if(counter <= 8)
			bit = g_nodebus_txdata & ((uint16_t)1<<counter);
		else /*if(counter == 9)*/
			bit = 1;
		
		if(bit)
			NODE_TX_PORT |= (1<<NODE_TX_BIT);
		else
			NODE_TX_PORT &= ~(1<<NODE_TX_BIT);
		
		if(counter >= 9){
			TIMSK0 &= ~(1<<TOIE0);
			g_timer0_state = NODEBUS_READY;
			nodebus_run_schedule();
		}
		else{
			counter++;
			g_nodebus_counter = counter;
		}
	}
	/*else if(g_timer0_state == NODEBUS_TXSTOP){
		g_nodebus_counter--;
		if(g_nodebus_counter <= 0){
			TIMSK0 &= ~(1<<TOIE0);
			g_nodebus_next_action();
		}
	}*/
	else if(g_timer0_state == NODEBUS_RXBUSY){
		int8_t counter = g_nodebus_counter;
		static uint16_t data = 0;
		/*NODE_TX_PORT &= ~(1<<NODE_TX_BIT);
		_delay_us(10);
		NODE_TX_PORT |= (1<<NODE_TX_BIT);*/
		if(counter == -1){
			// If start bit is high, it is a failure.
			if(NODE_RX_PORT_IN & (1<<NODE_RX_BIT)){
				nodebus_raw_rxbuf_insert(NODEBUS_DATA_RXFAIL);
				TIMSK0 &= ~(1<<TOIE0);
				g_timer0_state = NODEBUS_READY;

				// Cancel all scheduled receives
				g_nodebus_schedule_raw_receives = 0;
			}
			else{
				data = 0;
			}
		}
		else if(counter <= 8){
			if(NODE_RX_PORT_IN & (1<<NODE_RX_BIT)){
				//NODE_TX_PORT |= (1<<NODE_TX_BIT);
				data |= (uint16_t)1<<counter;
			}
			else{
				//NODE_TX_PORT &= ~(1<<NODE_TX_BIT);
			}
		}
		else{
			// If stop bit is low, it is a failure.
			if(!(NODE_RX_PORT_IN & (1<<NODE_RX_BIT))){
				nodebus_raw_rxbuf_insert(NODEBUS_DATA_RXFAIL);
				TIMSK0 &= ~(1<<TOIE0);
				g_timer0_state = NODEBUS_READY;

				// Cancel all scheduled receives
				g_nodebus_schedule_raw_receives = 0;
			}
		}

		if(counter >= 9){
			TIMSK0 &= ~(1<<TOIE0);
			g_timer0_state = NODEBUS_READY;
			nodebus_raw_rxbuf_insert(data);
			nodebus_run_schedule();
		}
		else{
			counter++;
			g_nodebus_counter = counter;
		}
	}
	else if(g_timer0_state == NODEBUS_RXWAIT){
		// An RX timeout
		nodebus_raw_rxbuf_insert(NODEBUS_DATA_RXFAIL);
		// Disable timer0 ovf interrupt
		TIMSK0 &= ~(1<<TOIE0);
		// Disable INT1 interrupt
		EIMSK |= (1<<INT1);
		g_timer0_state = NODEBUS_READY;

		// Cancel all scheduled receives
		g_nodebus_schedule_raw_receives = 0;
	}
}

ISR(INT1_vect)
{
	if(g_timer0_state == NODEBUS_RXWAIT){
		// Disable INT1 interrupt
		EIMSK |= (1<<INT1);
		
		g_nodebus_counter = -1;
		g_timer0_state = NODEBUS_RXBUSY;

		timer0_div(8);
		// Clear timer0 overflow interrupt flag
		TIFR0 |= (1<<TOV0);
		// Make a timer0 overflow interrupt in half a bit
		TCNT0 = 0x100 - NODEBAUD_TIMER0_BYTELEN/2;
		// Enable timer0 overflow interrupt
		TIMSK0 |= (1<<TOIE0);
	}
}

void node_raw_transmit_start(uint16_t data)
{
	g_nodebus_txdata = data;
	g_nodebus_counter = -1;
	g_timer0_state = NODEBUS_TXBUSY;
	timer0_div(8);
	// Clear overflow interrupt flag
	TIFR0 |= (1<<TOV0);
	// Make an overflow interrupt in one timer cycle
	TCNT0 = 0xff;
	// Enable interrupt
	TIMSK0 |= (1<<TOIE0);
}

void node_raw_transmit_end(void)
{
	while(g_timer0_state != NODEBUS_READY);
}

/*void node_raw_transmit(uint16_t data)
{
	node_raw_transmit_start(data);
	node_raw_transmit_end();
}*/

inline uint8_t node_raw_receive_start(void)
{
	cli();

	// Fail if RX is not at idle state (=1)
	if(!(NODE_RX_PORT_IN & (1<<NODE_RX_BIT))){
		sei();
		return 1;
	}

	// Set INT1 to trigger on low level
	EICRA &= ~(1<<ISC11) & ~(1<<ISC10);
	// Enable INT1 interrupt
	EIMSK |= (1<<INT1);

	// Set timer0 to trigger at the end of timeout
	g_timer0_state = NODEBUS_RXWAIT;
	timer0_div(64);
	TCNT0 = 0x100 - TIMER_US(F_CPU/64, 1000);
	// Clear overflow interrupt flag
	TIFR0 |= (1<<TOV0);
	// Enable interrupt
	TIMSK0 |= (1<<TOIE0);

	sei();

	return 0;
}

/*
	The received data is in g_nodebus_raw_rxbuf.

	returns 0 if all data is good, 1 otherwise.
*/
uint8_t node_raw_receive_end(void)
{
	while(g_timer0_state != NODEBUS_READY);
	
	uint8_t i;
	uint8_t l = g_nodebus_raw_rxbuf_len;
	for(i=0; i<l; i++){
		if(g_nodebus_raw_rxbuf[i] == NODEBUS_DATA_RXFAIL){
			return 1;
		}
	}
	return 0;
}

/*uint8_t node_raw_receive(uint16_t *dataptr)
{
	if(node_raw_receive_async()){
		return 1;
	}
	
	return node_raw_receive_end(dataptr);
}*/

/*
	Return value:
		0: completed
		1: timeout
		2: verification error
*/
static uint8_t node_transmit(uint16_t data)
{
	g_nodebus_schedule_raw_receives = 1;
	g_nodebus_raw_rxbuf_len = 0;
	node_raw_transmit_start(data);
	if(node_raw_receive_end()){
		return 1;
	}
	if(g_nodebus_raw_rxbuf[0] != data){
		return 2;
	}
	node_raw_transmit_start(MOSI_ACK);
	node_raw_transmit_end();
	return 0;
}

void node_address(uint8_t address)
{
	// Send address
	node_transmit((1<<8)|address);
	_delay_us(NODEBUS_TX_SPACING);
}

/*
	return value:
		0: success
		1: timeout error
		2: verification error
	
	cmdlen: number of command bytes
	cmd: command bytes, sent after address
	replylen: number of reply bytes
	replybuf: reply bytes go here after receiving
	zero_ends: if non-zero, receiving is ended if 0 is received.
		Also, receiving is stopped so that a zero can be added if buffer fills.
*/
uint8_t node_command(uint8_t cmdlen, uint8_t *cmd,
		uint8_t replylen, uint8_t *replybuf, uint8_t zero_ends)
{
	uint8_t cmd_i;
	
	// Send all command bytes except the last one
	for(cmd_i=0; cmd_i < cmdlen - 1; cmd_i++){
		node_transmit(cmd[cmd_i]);
		_delay_us(NODEBUS_TX_SPACING);
	}

	// Schedule receive for verification of last command
	g_nodebus_schedule_raw_receives = 1;
	g_nodebus_raw_rxbuf_len = 0;
	// Transmit last command
	node_raw_transmit_start(cmd[cmd_i]);
	// Wait for transmit and receive
	if(node_raw_receive_end())
		return 1;
	// Validate received verification
	if(g_nodebus_raw_rxbuf[0] != cmd[cmd_i])
		return 2;
	/*
		If there will be no replies, just transmit an ack and quit.
	*/
	if(replylen == 0){
		// Transmit ACK for last command
		node_raw_transmit_start(MOSI_ACK);
		// Wait for transmit
		node_raw_transmit_end();
		_delay_us(NODEBUS_TX_SPACING);
		return 0;
	}

	/*
		So there will be replies...
	*/
	
	// Schedule receive for reply 0
	g_nodebus_schedule_raw_receives = 1;
	g_nodebus_raw_rxbuf_len = 0;
	// Transmit ACK for last command
	node_raw_transmit_start(MOSI_ACK);
	// Wait for transmit and receive
	if(node_raw_receive_end())
		return 1;
	// Reply 0 is in buffer.
	uint8_t reply = g_nodebus_raw_rxbuf[0];
	// The number of the reply in question, starting from 0.
	uint8_t reply_i;
	for(reply_i=0; reply_i < replylen - 1 - (zero_ends?1:0); reply_i++){
		// Schedule receive for ack and an another reply.
		if(zero_ends && reply == 0){
			break;
		}
		g_nodebus_schedule_raw_receives = 2;
		g_nodebus_raw_rxbuf_len = 0;
		// Transmit verification
		node_raw_transmit_start(reply);
		if(node_raw_receive_end())
			return 1;
		// Check ACK
		if(g_nodebus_raw_rxbuf[0] != MISO_ACK){
			return 2;
		}
		// Save processed reply to provided buffer
		replybuf[reply_i] = reply;
		// Read reply from nodebus buffer
		reply = g_nodebus_raw_rxbuf[1];
	}
	// Schedule receive for the last ack.
	g_nodebus_schedule_raw_receives = 1;
	g_nodebus_raw_rxbuf_len = 0;
	// Transmit verification
	node_raw_transmit_start(reply);
	if(node_raw_receive_end())
		return 1;
	// Check ACK
	if(g_nodebus_raw_rxbuf[0] != MISO_ACK)
		return 2;
	// Save processed reply to provided buffer
	replybuf[reply_i] = reply;
	if(zero_ends){
		reply_i++;
		replybuf[reply_i+1] = 0;
	}

	// LOL. Looking at the code, this is never going to get this far... 8)
	return 0;

	/*
		address + 3 command bytes + 3 reply bytes:

		(transmit address, receive verification
		transmit ack)
		(transmit command 0, receive verification
		transmit ack)
		(transmit command 1, receive verification
		transmit ack)
		transmit command 2, receive verification
		transmit ack, receive reply 0
		(transmit verification, receive ack, receive reply 1)
		(transmit verification, receive ack, receive reply 2)
		transmit verification, receive ack
	*/
	

	//temporary:
#if jfalksfjsdf
	uint8_t reply_i;
	for(reply_i=0; reply_i < replylen; reply_i++){

	g_nodebus_raw_rxbuf_len = 0;
	if(node_raw_receive_start()){
		return 1;
	}
	if(node_raw_receive_end()){
		return 1;
	}
	uint16_t data = g_nodebus_raw_rxbuf[0];
	g_nodebus_schedule_raw_receives = 1;
	g_nodebus_raw_rxbuf_len = 0;
	node_raw_transmit_start(data);
	if(node_raw_receive_end()){
		return 1;
	}
	if(g_nodebus_raw_rxbuf[0] != MISO_ACK){
		return 2;
	}
	*dataptr = data;
	return 0;
#endif
}

// baud in Hz
static void usart_init(uint16_t baud)
{
	// Set baud rate
	UBRR = F_CPU / 8 / baud - 1;
	// Baud rate divider 8
	UCSRA = (1<<U2X);
	// Set frame format: 8 data, no parity, 2 stop bits
	UCSRC = (0<<UMSEL0)|(0<<UPM0)|(0<<USBS)|(1<<UCSZ1)|(1<<UCSZ0);
	// Enable receiver and transmitter
	//UCSRB = (1<<RXEN)|(1<<TXEN);
	// Enable receiver and transmitter and RX interrupt
	//UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE);
	// Enable receiver and RX interrupt
	UCSRB = (1<<TXEN)|(1<<RXCIE);
}

/*
	Mainloop-asynchronous nodebus control
*/
#if 0
volatile uint8_t g_nodebus_node_i = 0;

enum nodebus_action
{
	A_FAILWAIT, // or startup
	A_VOLTAGE_ADDR,
	A_VOLTAGE_ADDRSTOP, // stop bits
	A_VOLTAGE_CMD,
	A_VOLTAGE_RCV,
}
volatile g_nodebus_action_i = A_FAILWAIT;

void nodebus_next_action(void)
{
	uint8_t node_i = g_nodebus_node_i;
	uint8_t node_n = node_i + 1;
	uint8_t action_i = g_nodebus_action_i;
	/*
		action_i represents the action that WAS DONE before executing
		this function.

		TODO: Lol fail, we have to send the ACKs and stuff, too...
	*/
	switch(action_i)
	{
	case A_FAILWAIT:
		// failwait doesn't fail, so don't check. Send address.
		node_raw_transmit_async((1<<8) | node_n);
		break;
	case A_VOLTAGE_ADDR:
		// tx doesn't fail either. Delay stop bits.
		node_raw_delay_async(2);
		break;
	case A_VOLTAGE_ADDRSTOP:
		// delay never fails. Send command
		node_raw_transmit_async(MOSI_GET_ERRORLEVEL);
		break;
	case A_VOLTAGE_CMD:
		// tx never fails. Receive reply.
	}

	g_nodebus_action_i++;
	return;
fail:
	node_raw_delay_async(13);
	g_nodebus_action_i = A_FAILWAIT;
	if(g_nodebus_node_i < g_cell_count - 1)
		g_nodebus_node_i++;
	else
		g_nodebus_node_i = 0;
}
#endif

/*
	USART
*/

inline void usart_rx_disable(void)
{
	UCSRB &= ~(1<<RXEN);
}

static void usart_rx_enable(void)
{
	UCSRB |= (1<<RXEN);
}

inline void usart_transmit(uint8_t data)
{
	while(!(UCSRA & (1<<UDRE)));
	UDR = data;
}

void usart_transmit_text(const char *p)
{
	if(p==NULL) return;
	if(p[0]==0) return;
	do{
		usart_transmit(*p);
	}while(*(++p));
}

void usart_transmit_text_P(const char *p)
{
	if(p==NULL) return;
	if(pgm_read_byte(&p[0])==0) return;
	do{
		usart_transmit(pgm_read_byte(p));
	}while(pgm_read_byte(++p));
}

// BEGIN SHELL COMMANDS

#define SHELL_COMMAND(command)\
	const char shell_ ## command ## _name[] PROGMEM = #command;\
	void shell_ ## command (const char *arg)

SHELL_COMMAND(version)
{
	usart_transmit_text_P(PSTR("version:" IDENTIFY_STRING "\r\n"));
}

void var_transmit_int32(PGM_P n, int32_t x)
{
	usart_transmit_text_P(PSTR("status:"));
	usart_transmit_text_P(n);
	usart_transmit('=');
	char buf[12];
	ltoa(x, buf, 10);
	usart_transmit_text(buf);
	usart_transmit_text_P(PSTR("\r\n"));
}

#define VAR_TRANSMIT_UINT8(n, x) var_transmit_int32(PSTR(n), x)
#define VAR_TRANSMIT_INT16(n, x) var_transmit_int32(PSTR(n), x)
#define VAR_TRANSMIT_INT32(n, x) var_transmit_int32(PSTR(n), x)

SHELL_COMMAND(status)
{
	//TODO: print some values that are instantaneously available
	VAR_TRANSMIT_UINT8("errorlevel", g_errorlevel);
	VAR_TRANSMIT_INT32("uptime", g_uptime_seconds);
	VAR_TRANSMIT_INT32("voltage_psu", g_voltage_psu);
	VAR_TRANSMIT_INT32("voltage_battery", g_voltage_battery);
	VAR_TRANSMIT_INT32("voltage_node_sum", g_node_voltage_sum);
	VAR_TRANSMIT_INT16("current_charger", g_charge_current);
	VAR_TRANSMIT_INT32("current_main", 0);
	VAR_TRANSMIT_INT32("mah_charger", g_mah_charger);
	VAR_TRANSMIT_UINT8("settings_saved", g_settings_changed == -1 ? 1 : 0);
	VAR_TRANSMIT_UINT8("is_charging", g_charging);
	VAR_TRANSMIT_UINT8("pwm_charger", ((psc_get()+1) * 100) / PWM_MAX);

	usart_transmit_text_P(PSTR("status:text="));
	usart_transmit_text_P((PGM_P)g_statustext);
	usart_transmit_text_P(PSTR("\r\n"));
	g_statustext = PSTR("");

	VAR_TRANSMIT_UINT8("comm_error_count", g_comm_error_count);
	VAR_TRANSMIT_UINT8("wanted_charge_current", g_wanted_charge_current);
}

/*SHELL_COMMAND(debug)
{
	VAR_TRANSMIT_INT32("psc_get()", psc_get());
}*/

SHELL_COMMAND(temp_calibrate)
{
	int16_t i = atol(arg);
	if(arg[0] == 0 || i < -128 || i > 126){
		usart_transmit_text_P(PSTR("protocol:parameters=temperature\r\n"));
		return;
	}
	g_temperature_calibration_requested = 1;
	g_temperature_calibration_value = i;
	//g_command_continues = 1;
}

SHELL_COMMAND(set)
{
	uint8_t namelen=0;
	// p is left to point at start of value
	const char *p = arg;
	while(p != 0){
		if(*p == ' '){
			p++;
			break;
		}
		if(*p == 0){
			break;
		}
		namelen++;
		p++;
	}
	// Find the variable and set/get it
	uint8_t num_shown = 0;
	struct setting *v = settings;
	for(; pgm_read_byte(&v->type) != 0; v++){
		/*usart_transmit_text_P(PSTR("'"));
		usart_transmit_text_P((PGM_P)pgm_read_word(&v->name));
		usart_transmit_text_P(PSTR("'"));
		usart_transmit_text(arg);
		usart_transmit_text_P(PSTR("'\r\n"));*/
		if((strlen_P((PGM_P)pgm_read_word(&v->name)) == namelen &&
				strncmp_P(arg, (PGM_P)pgm_read_word(&v->name), namelen)==0)
				|| arg[0]==0){
			if(*p != 0){
				// Set it
				switch(pgm_read_byte(&v->type)){
				case VARTYPE_uint8_t:
					*((uint8_t*)pgm_read_word(&v->ptr)) = atol(p);
					break;
				case VARTYPE_int8_t:
					*((int8_t*)pgm_read_word(&v->ptr)) = atol(p);
					break;
				case VARTYPE_uint16_t:
					*((uint16_t*)pgm_read_word(&v->ptr)) = atol(p);
					break;
				case VARTYPE_uint32_t:
					*((uint32_t*)pgm_read_word(&v->ptr)) = atol(p);
					break;
				default:
					usart_transmit_text_P(PSTR("error: set: unsupported settingtype\r\n"));
				}
				g_settings_changed = SETTINGS_CHANGED_COUNT;
			}

			// Get it
#define RETBUF_LEN 12 // enough for signed 32-bit + terminator
			char retbuf[RETBUF_LEN];
			switch(pgm_read_byte(&v->type)){
			case VARTYPE_uint8_t:
				utoa(*((uint8_t*)pgm_read_word(&v->ptr)), retbuf, 10);
				break;
			case VARTYPE_int8_t:
				ltoa(*((int8_t*)pgm_read_word(&v->ptr)), retbuf, 10);
				break;
			case VARTYPE_uint16_t:
				utoa(*((uint16_t*)pgm_read_word(&v->ptr)), retbuf, 10);
				break;
			case VARTYPE_uint32_t:
				ltoa(*((uint32_t*)pgm_read_word(&v->ptr)), retbuf, 10);
				break;
			default:
				retbuf[0] = 0;
			}
#undef RETBUF_LEN
			usart_transmit_text_P(PSTR("set:"));
			usart_transmit_text_P((PGM_P)pgm_read_word(&v->name));
			usart_transmit('=');
			usart_transmit_text(retbuf);
			usart_transmit_text_P(PSTR("\r\n"));

			num_shown++;
		}
	}
	if(num_shown == 0){
		usart_transmit_text_P(PSTR("error: setting not found\r\n"));
	}
}

SHELL_COMMAND(nodes)
{
	g_node_info_requested = 1;
	g_command_continues = 1;
}

SHELL_COMMAND(statusnodes)
{
	shell_status(NULL);
	shell_nodes(NULL);
}

SHELL_COMMAND(reset_mah_count)
{
	g_mah_charger = 0;
	g_reset_shunt_mah_requested = 1;
}

/*SHELL_COMMAND(psc_set)
{
	uint16_t a = atol(arg);
	char buf[7];
	utoa(a, buf, 10);
	usart_transmit_text(buf);
	usart_transmit_text_P(PSTR("\r\n"));
	psc_set(a);
}*/

// END SHELL COMMANDS

#define SHELL_COMMAND_ENTRY(command){\
	shell_ ## command,\
	shell_ ## command ## _name\
}

typedef void (*shell_func_t)(const char*);

struct shell_command
{
	shell_func_t func;
	PGM_P name; // in progmem
};

struct shell_command shell_commands[] PROGMEM =
{
	SHELL_COMMAND_ENTRY(version),
	SHELL_COMMAND_ENTRY(status),
	//SHELL_COMMAND_ENTRY(debug),
	SHELL_COMMAND_ENTRY(set),
	SHELL_COMMAND_ENTRY(nodes),
	SHELL_COMMAND_ENTRY(temp_calibrate),
	//SHELL_COMMAND_ENTRY(psc_set),
	SHELL_COMMAND_ENTRY(reset_mah_count),
	SHELL_COMMAND_ENTRY(statusnodes),
	{NULL, NULL}
};

static void prompt(void)
{
	usart_transmit('$');
}

#define USART_RX_BUF_SIZE 40
volatile char usart_rx_buf[USART_RX_BUF_SIZE];
volatile uint8_t usart_rx_buf_i = 0;
volatile uint8_t usart_line_received = 0;

static void handle_usart_line(void)
{
	usart_rx_buf[usart_rx_buf_i] = 0;

	/*usart_transmit_text_P(PSTR("'"));
	usart_transmit_text(usart_rx_buf);
	usart_transmit_text_P(PSTR("'\r\n"));*/
	
	uint8_t command_found = 0;
	if(usart_rx_buf_i != 0){
		// Find length of the name part (to first space)
		uint8_t namelen;
		for(namelen=0; namelen<usart_rx_buf_i; namelen++)
			if(usart_rx_buf[namelen] == ' '){
				usart_rx_buf[namelen] = 0;
				break;
			}
		// Find the function and run it
		struct shell_command *p = shell_commands;
		for(; pgm_read_word(&p->func) != 0; p++){
			if(strlen_P((PGM_P)pgm_read_word(&p->name)) == namelen &&
			strncmp_P((const char*)usart_rx_buf, (PGM_P)pgm_read_word(&p->name), namelen)==0){
				if(usart_rx_buf_i > namelen)
					namelen += 1;
				(*(shell_func_t)pgm_read_word(&p->func))((const char*)&(usart_rx_buf[namelen]));
				command_found = 1;
				break;
			}
		}
	}
	
	// If no command was found, say it
	if(command_found == 0){
		usart_transmit_text_P(PSTR("info: got '"));
		usart_transmit_text((const char*)usart_rx_buf);
		usart_transmit_text_P(PSTR("'\r\n"));
		usart_transmit_text_P(PSTR("protocol:commands="));
		struct shell_command *p = shell_commands;
		for(; pgm_read_word(&p->func) != 0; p++){
			usart_transmit_text_P((PGM_P)pgm_read_word(&p->name));
			usart_transmit(' ');
		}
		usart_transmit_text_P(PSTR("\r\n"));
	}
}

ISR(USART_RX_vect)
{
	uint8_t d = UDR;
	E_LED_PORT |= (1<<E_LED_BIT); // DEBUG
	_delay_us(5);
	E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG

	// backspace character
	if(d == 127){
		if(usart_rx_buf_i > 0){
			/*
				Use usart_transmit because it is inline.
				This saves us lots of pushes and pops at the beginning
				and the end of this interrupt.
			*/
			usart_transmit('\x1c');
			usart_transmit('\x08');
			usart_transmit(' ');
			usart_transmit('\x1c');
			usart_transmit('\x08');
			usart_rx_buf_i--;
		}
	}
	else
	// return character
	if(d == '\r'){
		usart_rx_disable();
		//usart_line_received = 1;

		sei();
		
		if(usart_rx_buf_i >= USART_RX_BUF_SIZE - 1){
			usart_transmit_text_P(PSTR("protocol:buffer_full\r\n"));
		}
		else{
			handle_usart_line();
		}

		// Reset buffer
		usart_rx_buf_i = 0;
		//usart_line_received = 0;
		
		if(g_command_continues == 0){
			usart_rx_enable();
			prompt();
		}
	}
	else if(d == '\n'){
		// Ignore
	}
	// other characters
	else{
		if(usart_rx_buf_i < USART_RX_BUF_SIZE - 1){
			usart_rx_buf[usart_rx_buf_i] = d;
			usart_rx_buf_i++;
		}
	}
}

// CHARGER STUFF

static void charger_start(void)
{
	/*
		1) Calculate a good starting level from g_voltage_psu and
		   g_voltage_battery.
		   - charge_voltage = g_voltage_battery * 3/4;
		   - pwm = charge_voltage / g_voltage_psu

		2) Start charging.
	*/
	
	//usart_transmit_text_P(PSTR("info: charger_start\r\n"));
	/*uint32_t charge_voltage = g_voltage_battery;
	uint16_t pwm = charge_voltage * PSC_MAX / g_voltage_psu;
	if(pwm > PWM_MAX) pwm = PWM_MAX;
	//debugprint(20, "setting pwm=%i\r\n", pwm);
	psc_set(pwm);*/
	psc_set(0);
	psc_start();
	g_wanted_charge_current = 0;
	g_charging = 1;
}

static void charger_stop(void)
{
	//usart_transmit_text_P(PSTR("info: charger_stop\r\n"));
	g_charging = 0;
	g_wanted_charge_current = 0;
	psc_stop();
	psc_set(0);
}

/*
	ADC control

	We have four channels:
	ADC_CH_BATTERY
	ADC_CH_PSU
	ADC_CH_CHARGE_SHUNT
	ADC_CH_SHUNT
*/

ISR(ADC_vect, ISR_NOBLOCK)
{
	static uint8_t g_adc_chan = 0xff;
	uint16_t a = ADC;
	sei();
	//E_LED_PORT |= (1<<E_LED_BIT); // DEBUG
	uint8_t chan = g_adc_chan;
	uint8_t next_chan;
	switch(chan)
	{
	case ADC_CH_BATTERY: {
		uint32_t new = VOLTAGE_SCALE * a;
		/*uint32_t v = g_voltage_battery;
		v -= v / 8;
		v += new / 8;*/
		if(g_charging){
			static uint8_t was_fatal = 0;
			if(new > g_cell_count * g_cell_voltage_max_fatal){
				if(was_fatal >= 2){
					/*int16_t pwm = psc_get();
					if(pwm < PWM_MAX/10)
						pwm = 0;
					else
						pwm -= PWM_MAX/10;*/
					charger_stop();
					g_statustext = PSTR("Stop chg; fatal voltage");
				}
				else{
					was_fatal++;
				}
			}
			else{
				was_fatal = 0;
			}
		}
		cli();
		g_voltage_battery = new;
		sei();
		next_chan = ADC_CH_PSU;
		break; }
	case ADC_CH_PSU: {
		uint32_t new = VOLTAGE_SCALE * a;
		/*uint32_t v = g_voltage_psu;
		v -= v / 8;
		v += new / 8;*/
		cli();
		g_voltage_psu = new;
		sei();
		next_chan = ADC_CH_CHARGE_SHUNT;
		break; }
	case ADC_CH_CHARGE_SHUNT: {
		if(a >= 512)
			a -= 1024;
		int16_t new = CHARGE_CURRENT_SCALE * a;
		if(g_charging){
			/*if(new > g_wanted_charge_current || new > CHARGE_CURRENT_MAX){
				// Too much current - decrease PWM
				uint16_t pwm = psc_get();
				if(pwm > 0)
					pwm -= 1;
				psc_set(pwm);
			}
			else{
				// Too little current - increase PWM
				uint16_t pwm = psc_get();
				if(pwm < PWM_MAX)
				//if(pwm < PWM_MAX)
					pwm += 1;
				psc_set(pwm);
			}*/
			int16_t wanted = g_wanted_charge_current;
			if(wanted > CHARGE_CURRENT_MAX)
				wanted = CHARGE_CURRENT_MAX;
			/*
				Calculate a difference that is relative to magnitude.
			*/
			int16_t diff = (wanted - new) / (abs(wanted)/8+20);
			//int16_t diff = (wanted - new) / 50;
			//if(diff > 4) diff = 4;
			//if(diff < -20) diff = -20;
			if(diff > 1) diff = 1;
			//if(diff < -20) diff = -20;
			int16_t pwm = psc_get();
			pwm += diff;
			if(pwm < 0)
				pwm = 0;
			else if(pwm > PWM_MAX)
				pwm = PWM_MAX;
			psc_set(pwm);
		}
		/*int16_t v = g_charge_current;
		v -= v / 4;
		v += new / 4;*/
		cli();
		g_charge_current = new;
		sei();
		next_chan = ADC_CH_SHUNT;
		break; }
	case ADC_CH_SHUNT:
		//TODO
		next_chan = ADC_CH_BATTERY;
		break;
	default:
		next_chan = ADC_CH_BATTERY;
	}
	g_adc_chan = next_chan;
	ADMUX = ADMUX_SETTINGS | next_chan;
	ADCSRA = ADCSRA_SETTINGS | (1<<ADSC);
	//E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG
}

// MAIN

uint8_t g_mcusr_mirror __attribute__ ((section (".noinit")));
void get_mcusr(void) \
	__attribute__((naked)) \
	__attribute__((section(".init3")));
void get_mcusr(void)
{
	g_mcusr_mirror = MCUSR;
	MCUSR = 0;
	wdt_disable();
}

int main(void)
{
	//char buf[12];
	/*
		This is a line-buffered macro hack for avoiding interrupts
		transmitting in the middle of a line at main loop.
	*/
/*#define TXBUF_SIZE 120
	static char txuf[TXBUF_SIZE] = {0};
#define tx(x){ strncat(txuf, x, TXBUF_SIZE); }
#define tx_P(x){ strncat_P(txuf, x, TXBUF_SIZE); }
#define tx_crlf(){\
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE){\
		usart_transmit_text(txuf);\
		usart_transmit_text_P(PSTR("\r\n"));\
		txuf[0] = 0;\
	}\
}*/

	/*
		The hack isn't needed at the moment as no data is sent
		asynchronously.
	*/
#define tx(x) usart_transmit_text(x)
#define tx_P(x) usart_transmit_text_P(x)
#define tx_crlf(x) usart_transmit_text_P(PSTR("\r\n"))

	wdt_enable(WDTO_8S);

	io_init();

	LED_PORT |= (1<<LED_BIT);
	
	usart_init(RS232_BAUD);

	usart_transmit_text_P(PSTR("\r\ninfo: bms55 (c) Perttu Ahola <celeron55@gmail.com>\r\n"));
	shell_version(NULL);
	//d_sendvar(((uint8_t)SPH<<8)+SPL, "sp");
	
	usart_transmit_text_P(PSTR("bootup:reset="));
	//if a watchdog timer reset occurred
	if(g_mcusr_mirror & (1<<WDRF)){
		usart_transmit_text_P(PSTR("watchdog\r\n"));
	}
	//if a brown-out reset occurred
	else if(g_mcusr_mirror & (1<<BORF)){
		usart_transmit_text_P(PSTR("brown-out\r\n"));
	}
	//if an external reset occurred
	else if(g_mcusr_mirror & (1<<EXTRF)){
		usart_transmit_text_P(PSTR("external\r\n"));
	}
	else{
		usart_transmit_text_P(PSTR("none\r\n"));
	}
	
	adc_init();

	//uint16_t charge_current_calibration = 512;
	//charge_current_calibration = adc_get_lowpassed(ADC_CH_CHARGE_CURRENT, 50);

	/*usart_transmit_text_P(PSTR("debug: charge_current_calibration="));
	toa(charge_current_calibration, buf, 10);
	usart_transmit_text(buf);
	usart_transmit_text_P(PSTR("\r\n"));*/

	psc_init();
	//psc_start();

	timer1_init();

	//psc_set(PSC_MAX / 2);
	//psc_set(0);
	//psc_set(97);
	/*psc_start();
	for(;;){
		wdt_reset();

		psc_set((97<<4)+0);
		_delay_ms(50);
		psc_set((97<<4)+8);
		_delay_ms(50);
		psc_set((97<<4)+16);
		_delay_ms(50);
		psc_set((97<<4)+24);
		_delay_ms(50);
	}*/

	//debugprint("PSC_MAX=%i\r\n", PSC_MAX);
	
	/*// DEBUG:
	for(;;){
		wdt_reset();
		psc_set(16);
		_delay_ms(2000);
		psc_set(24);
		_delay_ms(2000);
	}*/
	
	eeprom_load_settings();
	
	// Input is disabled until this is counted down to 0
	uint8_t ignore_input_counter = 10;
	// Enable interrupts, though (nodebus is interrupt-driven)
	sei();
	usart_rx_enable();
	prompt();

	#define CHARGER_RESTART_DELAY (1 + (40/(g_cell_count+1)))
	uint16_t charger_stopped_counter = CHARGER_RESTART_DELAY;

	uint32_t uptime_buf = 0; // in looptime
	int32_t charge_buf = 0; // in looptime * charge_current(mA)

	//BEGIN TODO: REMOVE

	/*charger_start();
	g_wanted_charge_current = 300;
	for(;;);*/

	//psc_set(PWM_MAX);
	//psc_set(100);
	//psc_start();
	//OCR0SB = 30;
	//psc_set(110);

	// END TODO

	for(;;){
		wdt_reset();

		// Debug jam - just seriously jam the whole thing.
		if(g_debug_jam){
			cli(); // Fuck interrupts
			for(;;); // :---)
		}
		
		uint16_t looptime = TCNT1;
		// Check overflow flag
		if(TIFR1 & (1<<TOV1))
			looptime = UINT16_MAX;
		TCNT1 = 0;
		// Clear overflow flag
		TIFR1 |= (1<<TOV1);

		uptime_buf += looptime;
		uint16_t uptime_buf_seconds = uptime_buf / TIMER1_CLOCK;
		g_uptime_seconds += uptime_buf_seconds;
		uptime_buf -= uptime_buf_seconds * TIMER1_CLOCK;
		
		charge_buf += (int32_t)g_charge_current * looptime;
		int32_t charge_buf_mah = charge_buf / (TIMER1_CLOCK * 3600);
		g_mah_charger += charge_buf_mah;
		charge_buf -= charge_buf_mah * (TIMER1_CLOCK * 3600);

		/*
			Handle USART
		*/
#if 0
		if(usart_line_received){
			/*
				If buffer is full, dump it. (we don't want lines that were cut
				at the middle.)
			*/
			if(usart_rx_buf_i >= USART_RX_BUF_SIZE - 1){
				usart_transmit_text_P(PSTR("protocol:buffer_full\r\n"));
			}
			else{
				handle_usart_line();
			}

			// Reset buffer
			usart_rx_buf_i = 0;
			usart_line_received = 0;
			
			if(g_command_continues == 0){
				usart_rx_enable();
				prompt();
			}
		}
#endif

		static uint8_t comm_error_count = 0;

		/*
			These variables are set when checking nodes and other
			stuff
		*/
		uint8_t errorlevel = 0;
		uint8_t low_voltage = 0;
		uint8_t high_shunting = 0;
		uint8_t any_shunt_zero = 0;
		uint32_t node_voltage_sum = 0;
		uint8_t comm_error = 0;
		
		// Cache some global variables
		uint8_t node_info_requested;
		uint8_t temperature_calibration_requested;
		uint8_t temperature_calibration_value;
		uint8_t reset_shunt_mah_requested;
		uint8_t cell_count;
		
		node_info_requested = g_node_info_requested;
		temperature_calibration_requested = g_temperature_calibration_requested;
		reset_shunt_mah_requested = g_reset_shunt_mah_requested;
		
		g_node_info_requested = 0;
		g_temperature_calibration_requested = 0;
		g_reset_shunt_mah_requested = 0;

		temperature_calibration_value = g_temperature_calibration_value;
		cell_count = g_cell_count;

		//g_node_info_requested_debug = node_info_requested;

		char buf[12];
		if(node_info_requested){
			utoa(cell_count, buf, 10);
			tx_P(PSTR("nodes:count="));
			tx(buf);
			tx_crlf();
		}
		uint8_t j;
		for(j=0; j<cell_count; j++){
			uint8_t i = j + 1;
			if(node_info_requested){
				utoa(i, buf, 10);
				tx_P(PSTR("node:n="));
				tx(buf);
				tx_P(PSTR(","));
			}

			node_address(i);

			/*
				Get and check errorlevel
			*/

#define NODE_COMM_ERROR(desc){ goto node_comm_error; }
//#define NODE_COMM_ERROR(desc){ d_sendtext(desc); goto node_comm_error; }
#define DEBUGI 1
			
			{
				uint8_t cmd = MOSI_GET_ERRORLEVEL;
				uint8_t errorlevel = 0;
				//if(i==DEBUGI) E_LED_PORT |= (1<<E_LED_BIT); // DEBUG
				if(node_command(1, &cmd, 1, &errorlevel, 0)){
					//if(i==DEBUGI) E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG
					NODE_COMM_ERROR("a1b");
				}
				//if(i==DEBUGI) E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG

				if(node_info_requested){
					tx_P(PSTR("errorlevel="));
					utoa(errorlevel, buf, 10);
					tx(buf);
					tx_P(PSTR(","));
				}
			}

			/*
				Get and check voltage
			*/
			
			{
				uint8_t cmd = MOSI_GET_VOLTAGE;
				uint8_t temp[2];
				uint16_t voltage;
				if(node_command(1, &cmd, 2, temp, 0)){
					NODE_COMM_ERROR("a2");
				}

				voltage = (uint16_t)temp[0] + ((uint16_t)temp[1] << 8);

				if(node_info_requested){
					tx_P(PSTR("volt="));
					utoa(voltage, buf, 10);
					tx(buf);
					tx_P(PSTR(","));
				}

				node_voltage_sum += voltage;

				// Check
				
				if(voltage < g_cell_voltage_min_fatal)
					RAISE(errorlevel, 2);
				if(voltage > g_cell_voltage_max_fatal)
					RAISE(errorlevel, 2);

				if(voltage < g_cell_voltage_min)
					low_voltage = 1;
			}

			/*
				Get and check temperature
			*/

			{
				uint8_t cmd = MOSI_GET_TEMPERATURE;
				int8_t temperature;
				if(node_command(1, &cmd, 1, (uint8_t*)&temperature, 0)){
					NODE_COMM_ERROR("a2");
				}
					
				if(node_info_requested){
					tx_P(PSTR("temp="));
					ltoa(temperature, buf, 10);
					tx(buf);
					tx_P(PSTR(","));
				}

				if(temperature < g_cell_temperature_min_fatal)
					RAISE(errorlevel, 2);
				if(temperature > g_cell_temperature_max_fatal)
					RAISE(errorlevel, 2);
			}

			/*
				Get and check shunt
			*/

			{
				uint8_t cmd = MOSI_GET_SHUNT_USAGE;
				uint8_t shunt;
				if(node_command(1, &cmd, 1, &shunt, 0)){
					NODE_COMM_ERROR("a3");
				}
					
				if(node_info_requested){
					tx_P(PSTR("shunt="));
					utoa(shunt, buf, 10);
					tx(buf);
					tx_P(PSTR(","));
				}

				if(shunt > 127)
					high_shunting = 1;
				if(shunt == 0)
					any_shunt_zero = 1;
			}

			/*
				Get and check shunt mAh
			*/
			{
				uint8_t cmd = MOSI_GET_SHUNT_MAH;
				uint8_t temp[2];
				uint16_t shunt_mah;
				if(node_command(1, &cmd, 2, temp, 0)){
					NODE_COMM_ERROR("a4");
				}

				shunt_mah = (uint16_t)temp[0] + ((uint16_t)temp[1] << 8);

				if(node_info_requested){
					tx_P(PSTR("shunt_mah="));
					utoa(shunt_mah, buf, 10);
					tx(buf);
					tx_P(PSTR(","));
				}
			}

			/*
				Get status text (only if client requested info)
			*/

			if(node_info_requested){
				uint8_t cmd = MOSI_GET_STATUS_TEXT;
				char text[MISO_STATUS_TEXT_MAX_LEN+1];
				//if(i==DEBUGI) E_LED_PORT |= (1<<E_LED_BIT); // DEBUG
				if(node_command(1, &cmd, MISO_STATUS_TEXT_MAX_LEN, (uint8_t*)text, 1)){
					//if(i==DEBUGI) E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG
					NODE_COMM_ERROR("a5");
				}
				//if(i==DEBUGI) E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG

				tx_P(PSTR("text="));
				tx(text);
				tx_P(PSTR(","));
			}

			/*
				 Reset shunt mAh counter if requested
			*/

			if(reset_shunt_mah_requested){
				uint8_t cmd = MOSI_RESET_SHUNT_MAH;
				if(node_command(1, &cmd, 0, 0, 0)){
					NODE_COMM_ERROR("a6");
				}
			}
			/*
				Calibrate temperature (if client requested)
			*/

			if(temperature_calibration_requested){
				uint8_t cmd[2];
				cmd[0] = MOSI_CALIBRATE_TEMPERATURE;
				cmd[1] = (uint8_t)temperature_calibration_value;
				if(node_command(2, cmd, 0, 0, 0)){
					NODE_COMM_ERROR("a7");
				}
			}

			/*
				Set maximum voltage
			*/

			uint8_t cmd[3];
			cmd[0] = MOSI_SET_VOLTAGE_MAX;
			cmd[1] = (uint8_t)(g_cell_voltage_max & 0x00ff);
			cmd[2] = (uint8_t)(g_cell_voltage_max >> 8);
			if(node_command(3, cmd, 0, 0, 0)){
				NODE_COMM_ERROR("a8");
			}

			/*
				Error handler
			*/

			if(node_info_requested){
				tx_P(PSTR("comm=1,"));
			}
			goto node_comm_error_skip;
node_comm_error:
			//E_LED_PORT |= (1<<E_LED_BIT); // DEBUG
			if(node_info_requested){
				tx_P(PSTR("comm=0,"));
			}
			comm_error = 1;
			_delay_us(NODEBUS_TX_SPACING*13);
			//E_LED_PORT &= ~(1<<E_LED_BIT); // DEBUG
node_comm_error_skip:
			if(node_info_requested){
				tx_crlf();
			}
		}

		if(comm_error){
			if(comm_error_count < 255)
				comm_error_count++;
		}
		else{
			comm_error_count = 0;
		}
		
#define COMM_ERROR_COUNT_ERROR 3
//#define COMM_ERROR_COUNT_ERROR 1 //DEBUG
		if(comm_error_count >= COMM_ERROR_COUNT_ERROR){
			RAISE(errorlevel, 2);
		}

		g_comm_error_count = comm_error_count;

		if(node_info_requested){
			g_command_continues = 0;
			usart_rx_enable();
			prompt();
		}

		/*uint32_t voltage_psu = adc_get_lowpassed_scale(ADC_CH_PSU, 10,  VOLTAGE_SCALE);
		uint32_t voltage_battery = adc_get_lowpassed_scale(ADC_CH_BATTERY, 100, VOLTAGE_SCALE);
		int16_t raw_current = adc_get_amp_lowpassed(ADC_CH_CHARGE_SHUNT, 10);
		int32_t charge_current = (int32_t)(raw_current * CHARGE_CURRENT_SCALE);*/
		
		/*ltoa(charge_current, buf, 10);
		//ltoa(raw_current, buf, 10);
		usart_transmit_text_P(PSTR("charge_current="));
		usart_transmit_text(buf);
		usart_transmit_text_P(PSTR("\r\n"));*/

		/*
			Check that the node voltages and the pack voltage match.
			If not, it is a fatal error.
		*/
		int32_t voltage_battery_best_guess;
		if(g_voltage_battery > node_voltage_sum - 3000
				&& g_voltage_battery < node_voltage_sum + 3000){
			g_pack_voltage_is_node_voltage_sum = 1;
			voltage_battery_best_guess = node_voltage_sum;
		}
		else{
			g_pack_voltage_is_node_voltage_sum = 0;
			if(comm_error == 0)
				RAISE(errorlevel, 2);
			voltage_battery_best_guess = g_voltage_battery;
		}

		if(low_voltage){
			RAISE(errorlevel, 1)
		}
		
		/*
			Copy to global variables
			IMPORTANT: everything that can set fatal_error should run
			before this.
		*/
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
			g_node_voltage_sum = node_voltage_sum;
			g_errorlevel = errorlevel;
		}
		
		static uint8_t relay_cutoff_counter = 0;

		if(errorlevel || low_voltage){
			RELAY_CTRL_PORT |= (1<<RELAY_CTRL_BIT);
			relay_cutoff_counter = 10;
		}
		else{
			if(relay_cutoff_counter == 0){
				RELAY_CTRL_PORT &= ~(1<<RELAY_CTRL_BIT);
			}
			else{
				relay_cutoff_counter--;
			}
		}

		/*
			How 'bout charging?
			First check that there are no errors.
			charge_on_error enables charging without checking errors
		*/

		if(charger_stopped_counter > 0)
			charger_stopped_counter--;
		
		//VAR_TRANSMIT_INT32("charger_stopped_counter", charger_stopped_counter);
		
		if((errorlevel != 0 && g_charge_on_error == 0) || charger_stopped_counter > 0){
			if(g_charging == 1){
				//usart_transmit_text_P(PSTR("error - stop\r\n"));
				charger_stop();
				charger_stopped_counter = CHARGER_RESTART_DELAY;
				if(errorlevel != 0){
					if(comm_error_count >= COMM_ERROR_COUNT_ERROR)
						g_statustext = PSTR("Stop chg; errorlevel(comm)");
					else
						g_statustext = PSTR("Stop chg; errorlevel");
				}
			}
		}
		else{
			/*
				If no charger is inserted, voltage_psu is is about 0.4V
				lower than voltage_battery.

				3 volts more serves as a trigger.
			*/
			if(g_charging == 0){
				if(g_voltage_psu > g_voltage_battery + 3000 && any_shunt_zero){
					charger_start();
				}
			}
			else{ // g_charging
				/*
					If too much voltage or current, decrease current
				*/
				int16_t wanted = g_wanted_charge_current;
#define CHG_CURRENT_INCREMENT (wanted/20+25)
//#define CHG_CURRENT_INCREMENT 25
				if(high_shunting
						|| any_shunt_zero == 0
						|| g_charge_current > (int16_t)g_charge_current_max
						|| voltage_battery_best_guess > g_cell_count * g_cell_voltage_max){
					//usart_transmit_text_P(PSTR("less\r\n"));
					if(g_wanted_charge_current > CHG_CURRENT_INCREMENT){
						g_wanted_charge_current -= CHG_CURRENT_INCREMENT;
					}
					else{
						g_wanted_charge_current = 0;
					}
				}
				else{
					//usart_transmit_text_P(PSTR("more\r\n"));
					if(g_wanted_charge_current < (int16_t)g_charge_current_max - CHG_CURRENT_INCREMENT){
						g_wanted_charge_current += CHG_CURRENT_INCREMENT;
					}
					else{
						g_wanted_charge_current = (int16_t)g_charge_current_max;
					}
				}
				
				//TODO: make minimum charge current settable
				if(psc_get() == PWM_MAX && g_charge_current < CHARGE_CURRENT_STOP){
					charger_stop();
					charger_stopped_counter = CHARGER_RESTART_DELAY;
					g_statustext = PSTR("Stop chg; low current");
				}
				/*else{
					// Can't raise PWM more - is there minimal current?
					//TODO: make this a setting
					if(g_charge_current <= 50){
					//if(raw_current <= 5){
						//usart_transmit_text_P(PSTR("can't raise - stop\r\n"));
						// Minimal current -> Stop charging.
						charger_stop();
						charger_stopped_counter = CHARGER_RESTART_DELAY;
						g_statustext = PSTR("Stop chg; low current");
					}
				}*/
				/*usart_transmit_text_P(PSTR("charge_current="));
				ltoa(charge_current, buf, 10);
				usart_transmit_text(buf);
				usart_transmit_text_P(PSTR(", psc_get()="));
				ltoa(psc_get(), buf, 10);
				usart_transmit_text(buf);
				usart_transmit_text_P(PSTR("\r\n"));*/
			}
		}

		if(g_charging)
			LED_PORT ^= (1<<LED_BIT);
		else
			LED_PORT |= (1<<LED_BIT);

		/*
			Count down cycles when settings have been changed and save
			after g_settings_changed counts to 0.
		*/
		if(g_settings_changed > -1){
			g_settings_changed--;
			if(g_settings_changed == 0){
				eeprom_write_settings();
				//usart_transmit_text_P(PSTR("info: settings saved\r\n"));
			}
		}
		
		// Enable interrupts and send '$' after a delay from boot
		if(ignore_input_counter > 0){
			ignore_input_counter--;
			if(ignore_input_counter == 0){
				//sei();
				/*usart_rx_enable();
				prompt();*/
			}
		}
	}

	return 0;
}