lw35-upgrade/printer/avr/main.c

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <avr/pgmspace.h>

#define BAUDRATE 19200
//#define DEBUG_BUILD DEBUG_BUILD

/* System functions */
/* Timers */
enum TIMERS{TIM_CARRIAGE,
	    TIM_WHEEL,
	    TIM_LINEFEED,
	    TIM_SYSTEM,
	    TIMS};

#define TIMESCALE 4
#define sleep_ms(channel, time_ms) sleep(channel, time_ms * TIMESCALE)
volatile uint16_t timer[TIMS];

#define set_timer(channel, time_systicks) {timer[channel] = time_systicks;}
uint8_t block_for(uint8_t channel, uint16_t time_systicks)
{
	if (timer[channel] == 0) {
		timer[channel] = time_systicks;
		return 1;
	}
	return 0;
}

ISR (TIMER1_COMPA_vect)
{
	uint8_t i;
	for (i = 0; i < TIMS; ++i) {
		if (timer[i] > 0) {
			timer[i]--;
		}
	}
}

/* Hardware Functions */
static inline void uart_irq_enable() {
	UCSRB |= (1 << RXCIE);
}

static inline void uart_irq_disable() {
	UCSRB &= ~(1 << RXCIE);
}

void uart_putc(char c)
{
	while (!(UCSRA & (1 << UDRE)));
	UDR = c;
}

#ifdef DEBUG_BUILD
void debug_putc(char c) {
	uart_putc(c);
};

void debug_write(char *c)
{
	while (*c) {
		uart_putc(*c);
		c++;
	}
}

unsigned int debug_printf(char *format,...)
{
	va_list args;
	unsigned int i;
	char printbuffer[64];

	va_start (args, format);

	i = vsprintf (printbuffer, format, args);
	va_end (args);

	debug_write(printbuffer);

	return i;
}
#else
void debug_write(char *c) {};
void debug_putc(char c) {};
unsigned int debug_printf(char *format,...) {return 0;};
#endif

void hardfault() {
	cli();
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xFF);
	uart_putc(0xAA);
	uart_putc(0x00);
	uart_putc(0x00);
	uart_putc(0x00);
	debug_write("HARDFAULT!\r\n");
	while(1);
}

/* Look-up-tables */
/* Daisy wheel */
#define PRINTER_CONTROL_CHAR 56
#define PRINTER_NO_CHAR 128

#define ASCII_TRANSLATION_TABLE_SIZE (128 + 12)
const uint8_t ascii_translation_table[ASCII_TRANSLATION_TABLE_SIZE] PROGMEM = {
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,

	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR, // ASCII 15

	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,

	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR,
	PRINTER_CONTROL_CHAR, // ASCII 31

	PRINTER_NO_CHAR, // Space

	52,	// !
	66,	// "
	44,	// #
	40,	// $
	29,	// %
	74,	// &
	58,	// '
	88,	// (
	86,	// )
	82,	// *
	84,	// +
	0,	// ,
	68,	// -

	1,	// .
	28,	// /
	38,	// 0
	30,	// 1
	32,	// 2
	31,	// 3
	33,	// 4
	34,	// 5
	35,	// 6
	36,	// 7
	37,	// 8
	39,	// 9

	94,	// :
	92,	// ;
	PRINTER_NO_CHAR, // <
	71,	// =
	PRINTER_NO_CHAR, // >

	64,	// ?
	PRINTER_NO_CHAR, // @
	76,	// A
	72,	// B
	67,	// C
	65,	// D
	70,	// E
	69,	// F
	63,	// G
	87,	// H
	90,	// I
	62,	// J
	85,	// K
	78,	// L
	95,	// M
	91,	// N
	61,	// O
	80,	// P
	59,	// Q
	57,	// R
	79,	// S
	77,	// T
	89,	// U
	73,	// V
	93,	// W
	83,	// X
	75,	// Y
	81,	// Z

	PRINTER_NO_CHAR, // [
	PRINTER_NO_CHAR, // BACKSLASH
	PRINTER_NO_CHAR, // ]
	PRINTER_NO_CHAR, // ^

	60,	// _
	54,	// `
	5,	// a
	15,	// b
	8,	// c
	21,	// d
	2,	// e
	16,	// f
	17,	// g
	12,	// h
	6,	// i
	26,	// j
	19,	// k
	11,	// l
	10,	// m
	14,	// n
	7,	// o
	13,	// p
	24,	// q
	3,	// r
	4,	// s
	9,	// t
	18,	// u
	20,	// v
	27,	// w
	25,	// x
	22,	// y
	23,	// z

	PRINTER_NO_CHAR, // {
	48,	// |
	PRINTER_NO_CHAR, // }
	PRINTER_NO_CHAR, // ~
	PRINTER_CONTROL_CHAR, // DEL

	45,	// ³
	47,	// ²
	50,	// ´
	51,	// §
	56,	// °
	41,	// ö
	42,	// ü
	43,	// ä
	46,	// Ö
	49,	// ß
	53,	// Ü
	55,	// Ä
};

/* DC drive */
#define MOTLIM(t) t##C
#define PIN_DCMOTOR (1 << 4)
#define PIN_LIMITSWITCH (1 << 5)

#define SOLENOID(t) t##B
#define PIN_CORRECTION (1 << 1)
#define PIN_ARMHAMMER (1 << 0)

/* Misc I/O */
#define MISCIO(t) t##D
#define PIN_BACKLIGHT (1 << 2)

/* Stepper drive */

/* Drive pattern:
 *   -A, +C
 *   -B, +D
 *   +A, -C
 *   +B, -D
 */

struct stepper_config {
	char port;
	uint8_t timch;
	uint8_t pin_a;
	uint8_t pin_b;
	uint8_t pin_c;
	uint8_t pin_d;
	uint8_t delay;
	uint16_t wraparound;
};

struct stepper_status {
	uint8_t step;
	int8_t dir;
	int8_t ldir;
	uint8_t lpstate;
	uint16_t pos; /* Absolute position */
	uint16_t target_pos;
};

/* Stepper configs */
#define REVERSAL_MULTIPLIER 32

#define STEPPER_CFG_HW_CARRIAGE(t) t##B
const struct stepper_config STEPPER_CFG_CARRIAGE = {
	.port = 'B',
	.timch = TIM_CARRIAGE,
	.pin_a = (1 << 5),
	.pin_b = (1 << 4),
	.pin_c = (1 << 3),
	.pin_d = (1 << 2),
	.delay = 3.5 * TIMESCALE,
	.wraparound = 0 /* No wraparound */
};

#define STEPPER_CFG_HW_WHEEL(t) t##D
const struct stepper_config STEPPER_CFG_WHEEL = {
	.port = 'D',
	.timch = TIM_WHEEL,
	.pin_a = (1 << 4),
	.pin_b = (1 << 5),
	.pin_c = (1 << 6),
	.pin_d = (1 << 7),
	.delay = 3.5 * TIMESCALE,
	.wraparound = 96 * 2
};

#define STEPPER_CFG_HW_LINEFEED(t) t##C
const struct stepper_config STEPPER_CFG_LINEFEED= {
	.port = 'C',
	.timch = TIM_LINEFEED,
	.pin_a = (1 << 2),
	.pin_b = (1 << 3),
	.pin_c = (1 << 0),
	.pin_d = (1 << 1),
	.delay = 3.5 * TIMESCALE,
	.wraparound = 0 /* No wraparound */
};

/* Stepper helpers */
#define STEPPER_CFG(NAME) STEPPER_CFG_##NAME
#define STEPPER_NEXT(NAME, DIR) stepper_next_f(&stepper_status_##NAME, \
	&STEPPER_CFG_##NAME, DIR)
#define STEPPER_SET_IO(NAME) STEPPER_CFG_HW_##NAME(DDR) = \
	stepper_calc_ioc(STEPPER_CFG_HW_##NAME(DDR), \
	&STEPPER_CFG_##NAME)
#define STEPPER_STOP(NAME) {STEPPER_NEXT(NAME, 0);}

/* Stepper vars */
struct stepper_status stepper_status_WHEEL = {
	.step = 0,
	.dir = -1,
	.lpstate = 0
};
struct stepper_status stepper_status_CARRIAGE = {
	.step = 0,
	.dir = -1,
	.lpstate = 0
};
struct stepper_status stepper_status_LINEFEED = {
	.step = 0,
	.dir = -1,
	.lpstate = 0
};

/* Stepper functions */
uint8_t stepper_calc_ioc(uint8_t pstate, const struct stepper_config *cfg)
{
	pstate |= cfg->pin_a | cfg->pin_b | cfg->pin_c | cfg->pin_d;
	return pstate;
}

void stepper_next_f(struct stepper_status *stat,
		    const struct stepper_config *cfg,
		    int8_t dir)
{
	uint8_t pstate = 0;

	/* Get current state of port */
	switch (cfg->port) {
	case 'B':
		pstate = PORTB;
		break;
	case 'C':
		pstate = PORTC;
		break;
	case 'D':
		pstate = PORTD;
		break;
	}

	/* Calculate next stepper state */
	if (stat->dir != 0) { /* Do not step for the recovery step */
		if (dir == 1 && stat->step == 3)
			stat->step = 0;
		else if (dir == -1 && stat->step == 0)
			stat->step = 3;
		else
			(stat->step) += dir;
	}

	/* Apply current state */
	if (dir == 0) {
		stat->lpstate = pstate;
		//if (cfg->port == 'B') debug_printf("[%d] stop at: %x\r\n", stat->step, pstate & 0xF);
		pstate &= ~(cfg->pin_a | cfg->pin_b | cfg->pin_c | cfg->pin_d);
	} else {
		if (stat->dir == 0) { /* If this is the first step */
			pstate |= stat->lpstate & (cfg->pin_a | cfg->pin_b | cfg->pin_c | cfg->pin_d);
			//if (cfg->port == 'B') debug_printf("[%d] restore to : %x\r\n", stat->step, pstate & 0xF);
		} else {
			switch (stat->step) {
			case 0:
				pstate &= ~cfg->pin_a;
				pstate |= cfg->pin_c;
				break;
			case 1:
				pstate &= ~cfg->pin_b;
				pstate |= cfg->pin_d;
				break;
			case 2:
				pstate |= cfg->pin_a;
				pstate &= ~cfg->pin_c;
				break;
			case 3:
				pstate |= cfg->pin_b;
				pstate &= ~cfg->pin_d;
				break;
			default:
				break;
			}
			//if (cfg->port == 'B') debug_printf("[%d] set to : %x\r\n", stat->step, pstate & 0xF);
		}
	}

	/* Update status information */
	if (stat->dir) { // Ignore recovery step
		if (stat->ldir == dir) { // Ignore reversal step
			stat->pos += dir;
			if (cfg->wraparound) {
				if (dir == 1) {
					if (stat->pos >= cfg->wraparound) {
						stat->pos = 0;
					}
				} else {
					if (stat->pos == 65535) { // Underflow
						stat->pos = cfg->wraparound - 1;
					}
				}

			}
		}
	}

	if (stat->dir)
		stat->ldir = stat->dir;
	stat->dir = dir;

	/* Set new state of port */
	switch (cfg->port) {
	case 'B':
		PORTB = pstate;
		break;
	case 'C':
		PORTC = pstate;
		break;
	case 'D':
		PORTD = pstate;
		break;
	}
}

int8_t stepper_required_direction(struct stepper_status *stat,
				  const struct stepper_config *cfg)
{
	int8_t dir = 0;
	uint16_t half;

	if (stat->pos == stat->target_pos) {
		return 0;
	} else if (stat->pos > stat->target_pos) {
		dir = -1;
	} else {
		dir = 1;
	}

	if (cfg->wraparound) {
		half = cfg->wraparound / 2;
		if ((stat->pos > half && stat->target_pos <= half) ||
				(stat->pos <= half && stat->target_pos > half)){
			dir *= -1;
		}
	}

	return dir;
}

uint8_t stepper_perform_movement(struct stepper_status *stat,
				 const struct stepper_config *cfg)
{
	int8_t dir;

	/* Check whether we are in an active movement state,
	   or whether we need to go into one.*/
	if (stat->dir != 0 || stat->pos != stat->target_pos) {
		/* Set up direction */
		if (block_for(cfg->timch, cfg->delay)) {
			dir = stepper_required_direction(stat, cfg);
			if ((dir == -1) && (stat->dir == 1)) { /* Reversal */
				dir = 0;
				//debug_write("[stp] Hard reversal detected. Pausing. -->|\r\n");
				set_timer(cfg->timch, cfg->delay * REVERSAL_MULTIPLIER);
			} else if ((dir == 1) && (stat->dir == -1)) { /* Reversal */
				dir = 0;
				//debug_write("[stp] Hard reversal detected. Pausing. |<--\r\n");
				set_timer(cfg->timch, cfg->delay * REVERSAL_MULTIPLIER);
			}
			stepper_next_f(stat, cfg, dir);
		}
	}
	return 0;
}

#define POSITION_REACHED(NAME) ((stepper_status_##NAME.dir == 0) && (stepper_status_##NAME.pos == stepper_status_##NAME.target_pos))

#define SET_TARGET(NAME, target) stepper_set_target(&stepper_status_##NAME, \
	&STEPPER_CFG_##NAME,\
	target)
#define SET_TARGET_DELTA(NAME, delta) SET_TARGET(NAME, \
	stepper_status_##NAME.target_pos - (delta))
void stepper_set_target(struct stepper_status *stat,
			const struct stepper_config *cfg,
			uint16_t target)
{
	if (cfg->wraparound) {
		while (target >= -cfg->wraparound) {
			debug_printf("WRAP- %u\r\n", target);
			target += cfg->wraparound;
		}
		while (target >= cfg->wraparound) {
			debug_printf("WRAP+ %u\r\n", target);
			target -= cfg->wraparound;
		}
	}
	stat->target_pos = target;
}

/* DC functions */
void arm_hammer()
{
	SOLENOID(PORT) |= PIN_ARMHAMMER;
	_delay_ms(30);
	SOLENOID(PORT) &= ~(PIN_ARMHAMMER);
}
void arm_correction()
{
	SOLENOID(PORT) |= PIN_CORRECTION;
	_delay_ms(30);
	SOLENOID(PORT) &= ~(PIN_CORRECTION);
}

#define DCMOTOR_EN MOTLIM(PORT) |= PIN_DCMOTOR
#define DCMOTOR_STOP MOTLIM(PORT) &= ~PIN_DCMOTOR
#define DCMOTOR_ISACTIVE (!!(MOTLIM(PORT) & PIN_DCMOTOR))
#define LIMITSWITCH (!(MOTLIM(PIN) & PIN_LIMITSWITCH))

#define BACKLIGHT_ON MISCIO(PORT) |= PIN_BACKLIGHT
#define BACKLIGHT_OFF MISCIO(PORT) &= ~PIN_BACKLIGHT

/* Communication Functions */
#define RINGBUF_CNT(wr,rd) ((wr) - (rd))
#define RINGBUF_FREE(wr,rd,sz) ((RINGBUF_CNT(rd,wr+1))) // TODO: 255 is actually not correct...
#define PRINT_BUFFER_SIZE 256 // Important: must be 256 atm, when changed, you need to change code handling print_buffer_*.
volatile uint8_t print_buffer[PRINT_BUFFER_SIZE];
volatile uint8_t print_buffer_wr = 0;
volatile uint8_t print_buffer_rd = 0;

#define UART_BYTE_READY (UCSRA & (1 << RXC))

ISR(USART_RXC_vect)
{
	if (UCSRA & (1 << RXC)) {
		print_buffer[print_buffer_wr++] = UDR;// WARNING: No overflow protection
		// Will auto wrap at 256; print_buffer_wr = print_buffer_wr % PRINT_BUFFER_SIZE;
	}
}

#define TX_BUFFER_SIZE 256 // Important: must be 256 atm, when changed, you need to change code handling print_buffer_*.
char tx_buffer[TX_BUFFER_SIZE];
uint8_t tx_buffer_wr = 0;
uint8_t tx_buffer_rd = 0;
uint8_t tx_buffer_async = 0;

uint8_t global_status;
#define STATUS_PAPERFEED (1 << 0)
#define STATUS_CARRIAGE (1 << 1)
#define STATUS_DAISY (1 << 2)
#define STATUS_PRINTHEAD (1 << 3)
#define STATUS_READY (1 << 4)
#define STATUSBIT_ACTIVE 5

void tx_buffer_putc(char c)
{
	if (tx_buffer_async) {
		if (tx_buffer_rd == tx_buffer_wr + 1) // implicit % 256
			hardfault();
		tx_buffer[tx_buffer_wr++] = c; // implicit % 256
	} else {
		uart_putc(c);
	}
}

void tx_status()
{
	unsigned char local_status = 0;
	local_status |= (DCMOTOR_ISACTIVE << STATUSBIT_ACTIVE);

	tx_buffer_putc(0xFF); /* Header[0] */
	tx_buffer_putc(0xAA); /* Header[1] */
	tx_buffer_putc((char) (global_status | local_status));
	tx_buffer_putc(RINGBUF_FREE(print_buffer_wr, print_buffer_rd, PRINT_BUFFER_SIZE));
	tx_buffer_putc(0); /* RESERVED */
	tx_buffer_putc(0); /* RESERVED */
}

void update_status(uint8_t status)
{
	global_status = status;

	tx_status();
}

void set_status(uint8_t bit)
{
	update_status(global_status | bit);
}

void clear_status(uint8_t bit)
{
	update_status(global_status & ~bit);
}

void tx_buffer_process()
{
	if (tx_buffer_rd != tx_buffer_wr) {
		if (UCSRA & (1 << UDRE)) {
			UDR = tx_buffer[tx_buffer_rd];
			tx_buffer_rd++; // implicit % 256
		}
	}
}

/* Initialization Functions */
void move_carriage_to_far_left(uint8_t reset)
{
	uint16_t cnt = 0;

	clear_status(STATUS_CARRIAGE);

	/* Init stepper controller */
	if (reset) {
		stepper_status_CARRIAGE.step = 0;
	}
	stepper_status_CARRIAGE.dir = -1;

	if (LIMITSWITCH) {
		debug_write("[car] Clearing switch area...\r\n");
		cnt = 32;
		while (LIMITSWITCH && (cnt > 0)) {
			if (block_for(TIM_CARRIAGE, STEPPER_CFG_CARRIAGE.delay)) {
				STEPPER_NEXT(CARRIAGE, 1);
				cnt--;
			}
		}
		if (LIMITSWITCH && !cnt) { // We're probably in correction mode
			DCMOTOR_EN;
			arm_correction();
			_delay_ms(1000);
			DCMOTOR_STOP;
		}
		if (LIMITSWITCH) { // If the switch is still not released, there's some problem we can't fix from SW
			hardfault();
		}
		for (cnt = 0; cnt < 16; cnt++) {
			STEPPER_NEXT(CARRIAGE, 1);
		}
		STEPPER_STOP(CARRIAGE);
		_delay_ms(100);
	}

	debug_write("[car] Moving carriage to far left...\r\n");
	cnt = 0;
	while (!LIMITSWITCH) {
		if (block_for(TIM_CARRIAGE, STEPPER_CFG_CARRIAGE.delay)) {
			cnt++;
			STEPPER_NEXT(CARRIAGE, -1);
		}

	}
	STEPPER_STOP(CARRIAGE);

	stepper_status_CARRIAGE.pos = 0;
	stepper_status_CARRIAGE.target_pos = 0;

	debug_printf("[car] Carriage left after %u steps.\r\n", cnt);
	set_status(STATUS_CARRIAGE);
}

void align_daisy_wheel()
{
	int i;

	debug_write("[whl] Aligning wheel...\r\n");
	clear_status(STATUS_DAISY);

	stepper_status_WHEEL.dir = 0;
	stepper_status_WHEEL.lpstate = 0;
	stepper_status_WHEEL.pos = 0;
	stepper_status_WHEEL.step = 0;
	stepper_status_WHEEL.target_pos = 0;
	stepper_status_WHEEL.ldir = 0;

	/* Rotate right for one revolution -> lock daisy wheel to assembly */
	for (i = 0; i < (96 + 1) * 2; ) {
		if (block_for(TIM_WHEEL, STEPPER_CFG_WHEEL.delay)) {
			STEPPER_NEXT(WHEEL, 1);
			i++;
		}
	}

	/* Rotate left for one revolution -> align daisy wheel assembly */
	for (i = 0; i < (96 + 1 + 6) * 2; ) {
		if (block_for(TIM_WHEEL, STEPPER_CFG_WHEEL.delay)) {
			STEPPER_NEXT(WHEEL, -1);
			i++;
		}
	}

	STEPPER_STOP(WHEEL);
	_delay_ms(STEPPER_CFG_WHEEL.delay * REVERSAL_MULTIPLIER);

	for (i = 0; i < 3;) {
		if (block_for(TIM_WHEEL, STEPPER_CFG_WHEEL.delay)) {
			STEPPER_NEXT(WHEEL, 1);
			i++;
		}
	}
	STEPPER_STOP(WHEEL);

	stepper_status_WHEEL.ldir = 1;
	stepper_status_WHEEL.target_pos = 0;
	stepper_status_WHEEL.pos = 0;

	debug_write("[whl] Alignment completed.\r\n");
	set_status(STATUS_DAISY);
}

void reset_printhead()
{
	debug_write("[hmr] Resetting printhead...\r\n");
	clear_status(STATUS_PRINTHEAD);
	DCMOTOR_EN;
	_delay_ms(200);
	DCMOTOR_STOP;
	debug_write("[hmr] Printhead reset completed.\r\n");
	set_status(STATUS_PRINTHEAD);
}

void initialize_paperfeed()
{
	int i;

	debug_write("[lfd] Initializing paperfeed...\r\n");
	clear_status(STATUS_PAPERFEED);

	for (i = 0; i < 10;) {
		if (block_for(TIM_LINEFEED, STEPPER_CFG_LINEFEED.delay)) {
			STEPPER_NEXT(LINEFEED, 1);
			i++;
		}
	}
	STEPPER_STOP(LINEFEED);

	stepper_status_LINEFEED.dir = 0;
	stepper_status_LINEFEED.lpstate = 0;
	stepper_status_LINEFEED.pos = 32768;
	stepper_status_LINEFEED.step = 0;
	stepper_status_LINEFEED.target_pos = 32768;
	stepper_status_LINEFEED.ldir = 0;

	debug_write("[lfd] Initialization completed.\r\n");
	set_status(STATUS_PAPERFEED);
}

/* Printer Functions */
#define PRINT_CHARACTER_WIDTH 10
#define PRINT_LINE_HEIGHT 34
#define PRINTER_STOP_RIGHT 1100

enum printer_mode_t {MANUAL, INTELLIGENT};

#define GET_PRINT_CODE(c) (pgm_read_byte(&ascii_translation_table[c]))

uint16_t print_margin_left = 100;
uint16_t print_margin_right = PRINTER_STOP_RIGHT - 100;

int8_t move_one_character(uint8_t backward)
{
	if (backward) {
		if (stepper_status_CARRIAGE.target_pos < print_margin_left + PRINT_CHARACTER_WIDTH) {
			return 1;
		}
		stepper_status_CARRIAGE.target_pos -= PRINT_CHARACTER_WIDTH;
	} else {
		if (stepper_status_CARRIAGE.target_pos >= print_margin_right - PRINT_CHARACTER_WIDTH) {
			return  1;
		}
		stepper_status_CARRIAGE.target_pos += PRINT_CHARACTER_WIDTH;
	}
	return 0;
}

void move_carriage_to_left_margin()
{
	stepper_status_CARRIAGE.target_pos = print_margin_left;
	debug_putc('\r');
	debug_putc('{');
}

/* Simulates the print of one line and returns the end position of the printhead */
/* It's only called once, so inline is good. */
inline int8_t simulate_print_run(uint8_t *start_end_index, uint8_t *recovery_index)
{
	uint8_t current_char;
	uint8_t index;
	uint16_t carriage_position;
	uint8_t translated;
	uint8_t status = 0;
	uint8_t line_matters = 0;

	index = *start_end_index;
	carriage_position = print_margin_left;

	while (!status) {
		current_char = print_buffer[index];

		if (index == print_buffer_wr) { /* END OF BUFFER - Can't determine length */
			status = 3;
			line_matters = 1;
			carriage_position = print_margin_right;
			break;
		}

		if (current_char == '\n') {
			index--;
			status = 1;
			break;

		} else if (current_char < ASCII_TRANSLATION_TABLE_SIZE) {
			translated = GET_PRINT_CODE(current_char);

			if (translated == PRINTER_CONTROL_CHAR) {
				// ignore

			} else {
				if (carriage_position >= print_margin_right - PRINT_CHARACTER_WIDTH) {
					status = 2;
					carriage_position += PRINT_CHARACTER_WIDTH;
					//(*recovery_index) = index + 1;
					break; /* Don't consume character */
				} else {
					carriage_position += PRINT_CHARACTER_WIDTH;
					if (translated != PRINTER_NO_CHAR) {
						line_matters = 1;
					}
				}
			}
		}

		index++; /* Consume character */
		(*recovery_index) = index + 1;
	}

	carriage_position -= PRINT_CHARACTER_WIDTH;
	*start_end_index = index;

	//debug_printf("$ restore to: %d M=%d S=%d\n", print_buffer[*recovery_index], line_matters, status);
	//debug_printf("CMP %d <= %d\n", ((carriage_position - print_margin_left) / 2) , (stepper_status_CARRIAGE.target_pos - print_margin_left));
	if (!line_matters) {
		return 0;
	} else if (((carriage_position - print_margin_left) / 2) < (stepper_status_CARRIAGE.target_pos - print_margin_left)) {
		//debug_printf("next char: %c\n", print_buffer[index]);
		//_delay_ms(5000);
		stepper_status_CARRIAGE.target_pos = carriage_position;
		return -1;
	} else {
		return 1;
	}
}

#define PRINT_COMMAND_RESET_ALL 0xFF
#define PRINT_COMMAND_HARDFAULLT 0xFE
#define PRINT_COMMAND_RESET_WHEEL 0xF0
#define PRINT_COMMAND_BACKLIGHT_ON 0xFD
#define PRINT_COMMAND_BACKLIGHT_OFF 0xFC
#define PRINT_COMMAND_SET_MARGIN_LEFT 0xE0
#define PRINT_COMMAND_SET_MARGIN_RIGHT 0xE1
#define PRINT_COMMAND_MOVE_CARRIAGE 0xE2
#define PRINT_COMMAND_MOVE_PAPERFEED 0xE3

enum printer_state {INIT, IDLE, HAMMER, PAUSED, SPECIAL_COMMAND};

// TODO: Special commands must currently not be used in reverse (auto) print mode
enum printer_state process_special_command(uint8_t command)
{
	uint16_t temp;
	uint8_t arg;

	switch (command) {
	case PRINT_COMMAND_RESET_ALL:
		return INIT;

	case PRINT_COMMAND_HARDFAULLT:
		hardfault();
		return INIT;

	case PRINT_COMMAND_BACKLIGHT_ON:
		BACKLIGHT_ON;
		return IDLE;

	case PRINT_COMMAND_BACKLIGHT_OFF:
		BACKLIGHT_OFF;
		return IDLE;

	case PRINT_COMMAND_RESET_WHEEL:
		stepper_status_CARRIAGE.target_pos = 0; // Go to far left to lock the daisy wheel
		DCMOTOR_STOP; // TODO: We only need to issue this and the line before once
		if (POSITION_REACHED(WHEEL) && POSITION_REACHED(CARRIAGE) && POSITION_REACHED(LINEFEED)) {
				align_daisy_wheel();
				return INIT;
		}
		break;

	case PRINT_COMMAND_SET_MARGIN_LEFT:
	case PRINT_COMMAND_SET_MARGIN_RIGHT:
		if (print_buffer_rd != print_buffer_wr) {
			temp = (uint16_t) print_buffer[print_buffer_rd];
			print_buffer_rd++;
			temp *= PRINT_CHARACTER_WIDTH;
			switch (command) {
			case PRINT_COMMAND_SET_MARGIN_LEFT:
				print_margin_left = temp;
				break;
			case PRINT_COMMAND_SET_MARGIN_RIGHT:
				print_margin_right = temp;
				break;
			}
			return INIT;
		}
		break;

	case PRINT_COMMAND_MOVE_CARRIAGE:
		if (print_buffer_rd != print_buffer_wr) {
			temp = (uint16_t) print_buffer[print_buffer_rd];
			print_buffer_rd++;
			temp *= PRINT_CHARACTER_WIDTH;
			stepper_status_CARRIAGE.target_pos = temp;
			return IDLE;
		}
		break;

	case PRINT_COMMAND_MOVE_PAPERFEED: // [line mult] [+/-] [6b abs amount]
		if (print_buffer_rd != print_buffer_wr) {
			arg = print_buffer[print_buffer_rd];
			print_buffer_rd++;
			temp = (uint16_t) (arg & 0x3F);
			if (arg & 0x40) {
				temp = -temp;
			}
			if (arg & 0x80) {
				temp *= PRINT_LINE_HEIGHT;
			}
			stepper_status_LINEFEED.target_pos += temp;
			return IDLE;
		}
		break;

	default:
		return IDLE;
	}

	return SPECIAL_COMMAND;
}


void printer_process()
{
	static enum printer_state state = INIT;
	static uint8_t backward = 0;
	static uint8_t print_buffer_backward = 0;
	static uint8_t print_buffer_recovery = 0;

	static uint8_t current_is_linebreak = 0;
	uint8_t current_char;
	static uint8_t special_command;
	uint8_t translated;

	uint8_t end_of_next_line = 0;
	int8_t temp;
	static uint8_t debug_character = 0;

	switch (state) {
	case IDLE:
		if (print_buffer_rd != print_buffer_wr) { // TODO: maybe add "|| current_is_linebreak"
			/* Fetch new character from buffer */
			if (!current_is_linebreak) {
				if (backward) {
					current_char = print_buffer[print_buffer_backward];
					print_buffer_backward--;
				} else {
					current_char = print_buffer[print_buffer_rd];
					print_buffer_rd++;
					tx_status(); // Tell SoC when bytes are consumed from the buffer. TODO: Too often?
				}
			} else {
				current_char = '\0';
			}

			/* Check whether the current character is a command or something to print */
			current_is_linebreak += (current_char == '\n');
			if (current_is_linebreak) {
				if (backward) {
					debug_putc('=');
					print_buffer_rd = print_buffer_recovery;
					tx_status();
				}
				backward = 0; // Re-evaluate backward. Default is forward.

				stepper_status_LINEFEED.target_pos += PRINT_LINE_HEIGHT;
				debug_putc('\n');
				current_is_linebreak--;

				/* Decide whether we move the carriage back to the left, or
				 * whether to print the new line in reverse. */

				end_of_next_line = print_buffer_rd;
				temp = simulate_print_run(&end_of_next_line, &print_buffer_recovery);

				if (temp == -1) {
					debug_putc('[');
					backward = 1;
					print_buffer_backward = end_of_next_line;

				} else if (temp == 0) {
					// do nothing
					debug_putc('#');

				} else {
					move_carriage_to_left_margin();
				}

			} else if (current_char < ASCII_TRANSLATION_TABLE_SIZE) {
				translated = GET_PRINT_CODE(current_char);
				debug_character = current_char;

				switch (translated) {
				case PRINTER_CONTROL_CHAR:
					break;

				case PRINTER_NO_CHAR:
					current_is_linebreak += move_one_character(backward);
					if (!current_is_linebreak)
						debug_putc(' ');
					break;

				default: /* It's a printable character */
					SET_TARGET(WHEEL, translated * 2);
					state = HAMMER;
					break;
				}
			} else { /* TODO: Special commands */
				special_command = current_char;
				state = SPECIAL_COMMAND;

				/* TODO: below are just notes */
				// process_command_forward(&modifier_state);
				// After this call, wait until a "return to normal op command".
				// I can also call this in simulate_print_run and return to end-of-line modifiers
				//   to allow BOLD en/dis using the toggle command if en/dis is split over multiple lines.
				// TODO: But how do I handle margin changes etc?
			}

			if (!DCMOTOR_ISACTIVE && state != SPECIAL_COMMAND) {
				DCMOTOR_EN;
				_delay_ms(100); /* Let the motor get up to speed */
			}
		} else {
			DCMOTOR_STOP;
		}
		break;

	case HAMMER:
		if (POSITION_REACHED(WHEEL) && POSITION_REACHED(CARRIAGE) && POSITION_REACHED(LINEFEED)) {
			arm_hammer();
			_delay_ms(50); // TODO: replace with timer
			debug_putc(debug_character);
			current_is_linebreak += move_one_character(backward);
			state = IDLE;
		}
		break;

	case SPECIAL_COMMAND:
		state = process_special_command(special_command);
		break;

	case INIT:
		move_carriage_to_left_margin();
		state = IDLE;
	}


	//	if (print_buffer_rd != print_buffer_wr) {
	//		if (UCSRA & (1 << UDRE)) {
	//			UDR = print_buffer[print_buffer_rd++];
	//		}
	//	}
	//	// debug only: load the CPU with some stepper movements
	//	pgm_read_byte(&ascii_translation_table[123]);
	//	stepper_status_CARRIAGE.target_pos += 10;
	//	stepper_status_WHEEL.target_pos += 10;
	//	// -----------

	if (stepper_status_CARRIAGE.target_pos > PRINTER_STOP_RIGHT ||
			stepper_status_CARRIAGE.target_pos < 0) { // Margin safety
		hardfault();
	}
	stepper_perform_movement(&stepper_status_LINEFEED, &STEPPER_CFG_LINEFEED);
	stepper_perform_movement(&stepper_status_CARRIAGE, &STEPPER_CFG_CARRIAGE);
	stepper_perform_movement(&stepper_status_WHEEL, &STEPPER_CFG_WHEEL);
}

void sync_uart()
{
	int i;
	for (i = 0; i < 10; ++i) {
		tx_buffer_putc(0xFF);
	}
}

/* Test Functions */
#ifdef DEBUG_BUILD
void systick_test()
{
	debug_write("[tmr] System timer test\r\n");
	while (1) {
		if (block_for(TIM_SYSTEM, 1000 * TIMESCALE)) {
			debug_write("tick.\r\n");
		}
		if (UCSRA & (1 << RXC)) {
			break;
		}
	}
}

static void system_test_printer()
{
	debug_write("[sys] Entering printer test mode\r\n");
	initialize_paperfeed();
	move_carriage_to_far_left(1);
	align_daisy_wheel();
	reset_printhead();

	uart_irq_enable();

	while (1) {
		printer_process();
	}
}

static void system_test_auto()
{
	char c;
	int do_it = 0;
	int print_stat = 0;
	static int goto_wheel = 0;

	debug_write("[sys] Entering system test mode\r\n");
	debug_write(">");

	while(1) {
		if (UCSRA & (1 << RXC)) {
			print_stat = 1;
			c = UDR;

			if (goto_wheel) {
				goto_wheel = 0;
				SET_TARGET(WHEEL, (GET_PRINT_CODE(c)) * 2);
			} else if (c >= '0' && c <= '9') {
				stepper_status_CARRIAGE.target_pos = 100 * (c - '0');
			} else {
				/* Linefeed control */
				switch (c) {
				case '=':
					stepper_status_LINEFEED.target_pos += 10;
					break;
				case '\\':
					if (stepper_status_LINEFEED.target_pos >= 10)
						stepper_status_LINEFEED.target_pos -= 10;
					break;
				default:
					break;
				}

				/* Carriage control */
				// Note: Characters 0 - 9 for carriage handled in if above
				switch (c) {
				case ' ':
					stepper_status_CARRIAGE.target_pos += 10;
					break;
				case 'z':
					if (stepper_status_CARRIAGE.target_pos >= 10)
						stepper_status_CARRIAGE.target_pos -= 10;
					break;
				case 'r':
					move_carriage_to_far_left(0);
					break;
				default:
					break;
				}

				/* DC component control (Hammer / correction / motor) */
				switch (c) {
				case 'h':
					DCMOTOR_EN;
					arm_hammer();
					_delay_ms(100); /* Note, this also locks the carriage movement -> important! */
					DCMOTOR_STOP;
					break;
				case 'H':
					arm_hammer();
					break;
				case 'c':
					arm_correction();
					break;
				case 'm':
					DCMOTOR_STOP;
					break;
				case 'M':
					DCMOTOR_EN;
					break;
				default:
					break;
				}

				/* Wheel control */
				switch (c) {
				case 'g':
					goto_wheel = 1;
					break;
				case '\'':
					//if (stepper_status_WHEEL.target_pos >= 2)
					SET_TARGET_DELTA(WHEEL, -2);
					debug_printf("[whl] New wheel: %d\r\n", stepper_status_WHEEL.target_pos);
					break;
				case ',':
					SET_TARGET_DELTA(WHEEL, 2);
					debug_printf("[whl] New wheel: %d\r\n", stepper_status_WHEEL.target_pos);
					break;
				case '"':
					//if (stepper_status_WHEEL.target_pos >= 2)
					SET_TARGET_DELTA(WHEEL, -60);
					debug_printf("[whl] New wheel: %d\r\n", stepper_status_WHEEL.target_pos);
					break;
				case '<':
					SET_TARGET_DELTA(WHEEL, 60);
					debug_printf("[whl] New wheel: %d\r\n", stepper_status_WHEEL.target_pos);
					break;
				case 'w':
					align_daisy_wheel();
					break;
				case 't':
					stepper_status_WHEEL.target_pos += 2;
					stepper_status_CARRIAGE.target_pos += 10;
					do_it = 1;
					break;
				default:
					break;
				}

				/* System control */
				switch (c) {
				case 'i':
					sei();
					debug_write("Interrupts enabled.\r\n");
					break;
				case 'I':
					cli();
					debug_write("Interrupts disabled.\r\n");
					break;
				case 't':
					systick_test();
					break;
				case 'f':
					hardfault();
					break;
				default:
					break;
				}

				/* Launch programs */
				switch (c) {
				case 'P':
					system_test_printer();
					break;
				}
			}
		}

		stepper_perform_movement(&stepper_status_CARRIAGE, &STEPPER_CFG_CARRIAGE);
		stepper_perform_movement(&stepper_status_WHEEL, &STEPPER_CFG_WHEEL);
		stepper_perform_movement(&stepper_status_LINEFEED, &STEPPER_CFG_LINEFEED);

		if (POSITION_REACHED(WHEEL) && POSITION_REACHED(CARRIAGE) && POSITION_REACHED(LINEFEED) && print_stat) {
			print_stat = 0;
			debug_printf("[pos] %uL %uC%c %uW\r\n",
			             stepper_status_LINEFEED.pos,
			             stepper_status_CARRIAGE.pos,
			             (LIMITSWITCH ? '*' : '+'),
			             stepper_status_WHEEL.pos);
		}

		if (do_it) {
			if (stepper_status_CARRIAGE.pos == stepper_status_CARRIAGE.target_pos) {
				do_it = 0;
				DCMOTOR_EN;
				arm_hammer();
				_delay_ms(100);
				DCMOTOR_STOP;
			}
		}
	}
}
#endif

void mainloop()
{
	tx_buffer_async = 1;
	uart_irq_enable(); /* rx buffer process */
	set_status(STATUS_READY);

	while (1) {
		printer_process();
		tx_buffer_process();
	}
}

int main()
{
	/* Disable interrupts */
	cli();

	/* Pre-init I/O */
	DDRB = 0;
	DDRC = 0;
	DDRD = 0;
	PORTB = 0;
	PORTC = 0;
	PORTD = 0;

	/* Set up UART */
	UCSRA = 0;
	UCSRB = (1 << TXEN) | (1 << RXEN);
	UCSRC = 0;
	UBRRH = ((((F_CPU / BAUDRATE) / 16) - 1) >> 8);
	UBRRL = (((F_CPU / BAUDRATE) / 16) - 1);

	/* Set up DC components */
	MOTLIM(DDR) |= PIN_DCMOTOR;
	MOTLIM(DDR) &= ~(PIN_LIMITSWITCH);
	MOTLIM(PORT) |= PIN_LIMITSWITCH; /* Pullup for limit switch */
	SOLENOID(DDR) |= PIN_ARMHAMMER;
	SOLENOID(DDR) |= PIN_CORRECTION;

	/* Set up steppers */
	STEPPER_SET_IO(CARRIAGE);
	STEPPER_SET_IO(WHEEL);
	STEPPER_SET_IO(LINEFEED);

	/* Set up miscellaneous I/O */
	MISCIO(DDR) |= PIN_BACKLIGHT;
	BACKLIGHT_ON;

	/* Set up SysTick Timer */
	TCCR1B = (1 << WGM12) | (1 << CS11); // f_tim = 8 MHz / 8
	OCR1A = 2000 / TIMESCALE;
	TIMSK = (1 << OCIE1A);

	/* Init system */
	sync_uart();
	update_status(0);
	debug_write("\n\n\r[sys] STARTING IO CONTROLLER...\r\n");
#ifndef DEBUG_BUILD
	while (!((UCSRA & (1 << RXC)))) {
		if (process_special_command(UDR) == INIT)
			break;
	}
	update_status(0);
	_delay_ms(250);
#endif
	debug_write("[sys] Enabling interrupts.\r\n");
	sei();

	/* Align printer */
	initialize_paperfeed();
	move_carriage_to_far_left(1);
	align_daisy_wheel();
	reset_printhead();

	debug_write("[sys] Startup completed.\r\n");

	/* Run system */
#ifdef DEBUG_BUILD
	system_test_auto();
#else
	mainloop();
#endif

	debug_write("[sys] REACHED END OF MAIN. HALTING.\r\n");
	while (1);
}