/*
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>. 
*/

#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "spi.h"
#include "serial.h"

uint8_t from_hex(char hex) {
	if(hex >= '0' && hex <= '9') return hex - '0';
	if(hex >= 'A' && hex <= 'F') return hex - 'A' + 10;
	if(hex >= 'a' && hex <= 'f') return hex - 'a' + 10;
	return 0;
}

bool spi_write(uint8_t address, void *p_data_in, uint8_t bytes) {
	char buf[515];
	uint8_t *p_data = p_data_in;
	sprintf(buf, "W%02X", address);
	while(bytes--) sprintf(buf, "%s%02X", buf, *p_data++);
	sprintf(buf, "%s\n", buf);
	swrite(buf, strlen(buf));
	char c;
	uint16_t ix = 0;
	while(1) {
		if(sread(&c, 1)) {
			if(c == '\n') {
				if(ix < 512 && !memcmp(&buf[0], "Sent ", 5)) return true;
				ix = 0;
			} else if(ix < 512) buf[ix++] = c;
		}
	}
	return false;
}

bool spi_read(uint8_t address, void *p_data_in, uint8_t bytes) {
	char buf[515];
	uint8_t *p_data = p_data_in;
	sprintf(buf, "R%02X%02X\n", address, bytes);
	swrite(buf, strlen(buf));
	char c;
	uint16_t ix = 0;
	while(1) {
		if(sread(&c, 1)) {
			if(c == '\n') {
				if(ix < 512 && !memcmp(&buf[2], " ->", 3)) {
					if(buf[5] == '\n') return false;
					buf[ix++] = c;
					ix = 6;
					while(buf[ix] != '\n') {
						if((ix % 3) == 0) *p_data = from_hex(buf[ix++]) << 4;
						else if((ix % 3) == 1) *p_data++ |= from_hex(buf[ix++]);
						else ix++;
					}
					return true;
				}
				ix = 0;
			} else if(ix < 512) buf[ix++] = c;
		}
	}
}

bool spi_getflags(uint8_t *p_flags) {
	char buf[515];
	sprintf(buf, "I\n");
	swrite(buf, strlen(buf));
	char c;
	uint16_t ix = 0;
	while(1) {
		if(sread(&c, 1)) {
			if(c == '\n') {
				if(ix < 512 && !memcmp(&buf[0], "Flags: ", 7)) {
					*p_flags = (from_hex(buf[7]) << 4) | (from_hex(buf[8]));
					return true;
				}
				ix = 0;
			} else if(ix < 512) buf[ix++] = c;
		}
	}
}

bool spi_init() {
	sdelay(250);
	swrite("\nO00\n", 5);
	spi_write(0x00, "\x81", 1);     // Clear IDENT mode
	uint8_t flags;
	return spi_getflags(&flags);
}

void spi_setflags(uint8_t flags) {
	char buf[6];
	sprintf(buf, "\nO%02X\n", flags);
	swrite(buf, 5);
}

void spi_delay() {
	sdelay(125);
}
