/* this file is for functions that produce an output */
#include "common.h"
#include "output.h"
#include "sw_spi.h"
#include <pwm.h>
#include <timers.h>
#include <delays.h>

#include "chars.h"
#define REG_SELECT LATCbits.LATC1
#define DISP_RESET LATCbits.LATC6
/* set up registers etc for LEDs and Laser */

unsigned char laser;

#define REVERSE_BITS(n) \
 n = ((n >> 1) & 0x55) | ((n << 1) & 0xaa);\
 n = ((n >> 2) & 0x33) | ((n << 2) & 0xcc);\
 n = ((n >> 4) & 0x0f) | ((n << 4) & 0xf0);\
	
void initialise_output() {
	OpenTimer2(TIMER_INT_OFF & T2_PS_1_16 & T2_POST_1_1);
	OpenPWM2(0x40);
	// Need to change stuff around, because OpenPWM2 does bad things
	TRISB = TRISB & 0b11010111;
	OpenSWSPI();
	LATBbits.LATB3 = 1;
	}

/* turn laser on and off */
void set_laser_brightness(void) {
	if (conf.laser) {
		CCP2CON=0b00001100;
	} else {
		CCP2CON=0;
		LATBbits.LATB3 = 1;
	}
	SetDCPWM2(0xff - conf.laser);
}

void write_char(char code, char invert) {
	rom char * data = &charset[0];
	unsigned char f;
	unsigned int index = (int)(code - 0x20) * 5; /* this gives us our offset into the charset */
	unsigned char temp;
	for (f = 0; f < 5; ++f) {
		if (invert) {
			temp = data[index + (4 - f)];
			/* reverse the bits here */
			temp = ((temp & 0b00001000) | ((temp & 0b00000111) << 4) | ((temp & 0b01110000) >> 4));
			temp = ((temp & 0b00101010) | ((temp & 0b00010001) << 2) | ((temp & 0b01000100) >> 2));
		} else {
			temp = data[index + f];
		}
		WriteSWSPI(temp);	
	}
}

/* write the four chars from text to the display,
 optionally inverted */
void write_display(char* text, unsigned char invert) {
	unsigned char f;
	DISP_RESET = 0;
	Delay10TCYx(100);
	DISP_RESET = 1;
	Delay10TCYx(100);

	/* write control data */
	set_display_brightness();
	/* write display data */
	Delay10TCYx(100);
	REG_SELECT = 0;
	Delay10TCYx(100);
	SW_SCK_PIN = 1;
	Delay10TCYx(100);
	ClearCSSWSPI();
	Delay10TCYx(100);
	for (f = 0; f <4; ++f) { 
		if (invert) 
			write_char(text[3-f],1);
		else 
			write_char(text[f],0);
	}
	Delay10TCYx(100);
	SetCSSWSPI();
	Delay10TCYx(100);
	SW_SCK_PIN = 0;
}

void set_display_brightness(void) {
	REG_SELECT = 1;
	Delay10TCYx(100);
	SW_SCK_PIN = 1;
	Delay10TCYx(100);
	ClearCSSWSPI();
	Delay10TCYx(100);
	WriteSWSPI((0x40 | conf.display) & 0x7F);
	Delay10TCYx(100);
	SetCSSWSPI();
	Delay10TCYx(100);
	SW_SCK_PIN = 0;
}