#include "utility.h"

#include "mem_registers.h"
#include "monitor_subroutines.h"
#include "line_data.h"

void num_to_decbuf(uint16_t n, uint8_t len, uint8_t *buf) {
	
	for(uint8_t idx = 0; idx < len; idx++) {
		buf[idx] = n % 10;
		n /= 10;
	}
}

// https://stackoverflow.com/questions/2602823/in-c-c-whats-the-simplest-way-to-reverse-the-order-of-bits-in-a-byte
uint8_t bit_reverse(uint8_t b) {
	b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
	b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
	b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
	
	return b;
}

// https://stackoverflow.com/questions/14009765/fastest-way-to-count-bits
uint8_t bit_count(uint8_t b) {
    b = (b & 0x55) + (b >> 1 & 0x55);
    b = (b & 0x33) + (b >> 2 & 0x33);
    b = (b & 0x0f) + (b >> 4 & 0x0f);
	
	return b;
}

void lfsr_init(uint16_t reg) {
	*((uint16_t*)LFSR_REGISTER_ADDRESS) = 0xF00D;
}

uint16_t lfsr_update(void) {
	uint16_t *lfsr = ((uint16_t*)LFSR_REGISTER_ADDRESS);

	*lfsr ^= (*lfsr) >> 7;
	*lfsr ^= (*lfsr) << 9;
	*lfsr ^= (*lfsr) >> 13;

    return *lfsr;
}

#define CRC8RDALLAS_POLY 0x31
uint8_t calculate_crc8(uint8_t* data, uint8_t len) {
	uint8_t crc = 0;
	
	for(uint8_t data_idx = 0; data_idx < len; data_idx++) {
		uint8_t carry;
		uint8_t d = data[data_idx]; 
		
		for (uint8_t i = 8; i > 0; i--) {
			carry = (crc & 0x80);
			crc <<= 1;
			if (d & 1) crc |= 1;
			d >>= 1;
			if (carry) crc ^= CRC8RDALLAS_POLY;		
		}	
	}
		
	return crc;
}