// CONFIG1
#pragma config FOSC = INTOSC    //  (INTOSC oscillator; I/O function on CLKIN pin)
#pragma config WDTE = ON        // Watchdog Timer Enable (WDT enabled)
#pragma config PWRTE = ON       // Power-up Timer Enable (PWRT enabled)
#pragma config MCLRE = OFF      // MCLR Pin Function Select (MCLR/VPP pin function is digital input)
#pragma config CP = OFF         // Flash Program Memory Code Protection (Program memory code protection is disabled)
#pragma config BOREN = ON       // Brown-out Reset Enable (Brown-out Reset enabled)
#pragma config CLKOUTEN = OFF   // Clock Out Enable (CLKOUT function is disabled. I/O or oscillator function on the CLKOUT pin)

// CONFIG2
#pragma config WRT = OFF        // Flash Memory Self-Write Protection (Write protection off)
#pragma config PLLEN = OFF      // PLL Enable (4x PLL disabled)
#pragma config STVREN = ON      // Stack Overflow/Underflow Reset Enable (Stack Overflow or Underflow will cause a Reset)
#pragma config BORV = LO        // Brown-out Reset Voltage Selection (Brown-out Reset Voltage (Vbor), low trip point selected.)
#pragma config LPBOREN = ON     // Low Power Brown-out Reset enable bit (LPBOR is enabled)
#pragma config LVP = OFF        // Low-Voltage Programming Enable (High-voltage on MCLR/VPP must be used for programming)

#include <xc.h>

// Clock frequency. Used by delay loops etc.
#define _XTAL_FREQ 16000000

#define ADC_CHANNEL 0

// Quiescent output voltage from the hall effect sensor. 0.6V
#define VQ 123
// Expected output from sensor: 85mV/A

void init()
{
	OSCCONbits.IRCF = 0b1111; // 16MHz clock.
	WDTCONbits.WDTPS = 0b01101; // 8s watchdog timer.
	VREGCONbits.VREGPM = 1; // Low-power sleep.

	// RA0 as analogue input (AN0)
	TRISAbits.TRISA0 = 1;
	ANSELAbits.ANSA0 = 1;

	ADCON1bits.ADFM = 1; // Right justified.
	ADCON1bits.ADCS = 0b101; // Set ADC clock to Fosc/16 for a 1µs Tad.
	ADCON1bits.ADPREF = 0b00; // Set ref to Vdd.

	// Set the remaining PORTA pins as digital outputs (except 3, which can only be an input).
	TRISAbits.TRISA1 = 0;
	TRISAbits.TRISA2 = 0;
	TRISAbits.TRISA4 = 0;
	TRISAbits.TRISA5 = 0;
}

uint16_t read_adc(const unsigned char channel)
{
	// Enable ADC
	ADCON0bits.CHS = channel;
	ADCON0bits.ADON = 1;

	// Wait for acquisition time.
	__delay_us(5);

	// Start conversion and wait until it's done.
	for(GO_nDONE = 1; GO_nDONE;){}

	// Disable it again
	ADCON0bits.ADON = 0;

	return ((uint16_t)ADRESH << 8) | ADRESL;
}

int main(void)
{
	uint16_t raw = 0;
	uint16_t raw_peak = 0;
	uint16_t count = 0;

	// Use a balanced Gray code so that each output flashes at a similar rate.
	// This looks better than having the LSB flash really fast and the MSB hardly change at all.
	uint8_t gray[32] = {0b10111, 0b10101, 0b10001, 0b11001, 0b11101, 0b01101, 0b01100, 0b01000, 0b01010, 0b11010, 0b11011, 0b10011, 0b10010, 0b10110, 0b10100, 0b00100, 0b00000, 0b10000, 0b11000, 0b11100, 0b11110, 0b11111, 0b01111, 0b01110, 0b00110, 0b00010, 0b00011, 0b01011, 0b01001, 0b00001, 0b00101, 0b00111};

	init();

	while (1)
	{
		CLRWDT();

		raw = read_adc(ADC_CHANNEL);

		if (raw > raw_peak)
		{
			raw_peak = raw;
		}

		count += (raw - VQ) * (0b0000001111111111 / (raw_peak - VQ));
		LATA = (uint8_t)(gray[count >> 11] << 1); // Shift by one because the outputs are RA1:5, skipping RA0.

		__delay_ms(25);
	}

	return EXIT_FAILURE;
}