#include "Arduino.h"
#include "AOI.h"
#include "SPI.h"
#include <Controllino.h>

#define SCK_PIN CONTROLLINO_PIN_HEADER_SCK
#define MOSI_PIN CONTROLLINO_PIN_HEADER_MOSI
#define MISO_PIN CONTROLLINO_PIN_HEADER_MISO

#define SETUP_TIME 10000UL
#define COMMS_CHECK_INTERVAL 3000UL
#define EXECUTION_CHECK_INTERVAL 1000UL

#define VALUE_COMMS_OK 0x5A		// (70)
#define VALUE_NOT_EXECUTED 0xAA	// (170)
#define VALUE_EXECUTED 0x55		// (85)
#define VALUE_NODATA 0xFE		// (254)

void AOI::init(uint8_t ssPin) {
	this->ssPin = ssPin;
	pinMode(this->ssPin, OUTPUT);
	digitalWrite(this->ssPin, HIGH);
}

void AOI::poll() {
	if ((millis() - commsCheckTimestamp) >= COMMS_CHECK_INTERVAL) {
		startCommand(COMMAND_COMMS_CHECK);
		commsCheckTimestamp = millis();
	}

	this->interfaceStateMachine();
}

uint8_t AOI::spiTransfer(uint8_t data) {
	if (DBG) {
		Serial.print("aoi send ");
		Serial.println(data);
	}

	SPI.end();

	pinMode(SCK_PIN, OUTPUT);
	pinMode(MOSI_PIN, OUTPUT);
	pinMode(MISO_PIN, INPUT);

    digitalWrite(this->ssPin, LOW);
	delayMicroseconds(SETUP_TIME);

	uint8_t received = 0;
	for(int8_t i = 7; i >= 0; i--) {
		digitalWrite(MOSI_PIN, (data>>i) & 1);
		delayMicroseconds(SETUP_TIME);
		digitalWrite(SCK_PIN, HIGH);
		received |= digitalRead(MISO_PIN) << i;
		delayMicroseconds(SETUP_TIME);
		digitalWrite(SCK_PIN, LOW);
	}
	
    digitalWrite(this->ssPin, HIGH);
	delayMicroseconds(SETUP_TIME);

	SPI.begin();

	return received;
}

bool AOI::startCommand(commands command) {
	if (interfaceState != INTERFACE_STATE_IDLE) return 0;

	switch (command) {
		case COMMAND_COMMS_CHECK:
			interfaceState = INTERFACE_STATE_GET_RESPONSE;
			responseLength = 1;
		break;
		case COMMAND_RECOGNIZE_SERIAL_NUMBER:
			interfaceState = INTERFACE_STATE_AWAITING_EXECUTION;
			responseLength = RESPONSE_MAX_LENGTH;
		break;
		default:
			return 0;
		break;
	}

	lastCommand = command;
	this->spiTransfer(command);
	commandDone = false;
	responseBufferCounter = 0;

	return 1;
}

void AOI::parseResponse() {
	if (DBG) {
		Serial.print("aoi parse response for command ");
		Serial.print(lastCommand);
		Serial.print(":");
		for (uint8_t i = 0; i < RESPONSE_MAX_LENGTH; i++) {
			Serial.print(" ");
			Serial.print(responseBuffer[i]);
		}
		Serial.println();
	}
	
	switch (lastCommand) {
		case COMMAND_COMMS_CHECK:
			if (responseBuffer[0] == VALUE_COMMS_OK) commsOk = true;
			else commsOk = false;
		break;
		case COMMAND_RECOGNIZE_SERIAL_NUMBER:
			lastSerialNumber = 0;
			for (uint8_t i = 0; i < RESPONSE_MAX_LENGTH; i++) {
				if (responseBuffer[i] == VALUE_NODATA) break;
				lastSerialNumber = lastSerialNumber * 10;
				lastSerialNumber += responseBuffer[i];
			}
		break;
	}
}

void AOI::error() {
	interfaceState = INTERFACE_STATE_IDLE;
	commsOk = false;

	switch (lastCommand) {
		case COMMAND_RECOGNIZE_SERIAL_NUMBER:
			lastSerialNumber = 0;
		break;
	}
}

void AOI::interfaceStateMachine() {
	switch (interfaceState) {
		case INTERFACE_STATE_IDLE:
		break;
		case INTERFACE_STATE_AWAITING_EXECUTION:
			// check if command has been executed every EXECUTION_CHECK_INTERVAL
			if ((millis() - executionCheckTimestamp) >= EXECUTION_CHECK_INTERVAL) {
				this->spiTransfer(COMMAND_CHECK_EXECUTION);
				interfaceState = INTERFACE_STATE_CHECK_EXECUTION;
				executionCheckTimestamp = millis();
			}
		break;
		case INTERFACE_STATE_CHECK_EXECUTION:
			switch (this->spiTransfer(COMMAND_GET_BYTE)) {
				case VALUE_NOT_EXECUTED:
					interfaceState = INTERFACE_STATE_AWAITING_EXECUTION;
				break;
				case VALUE_EXECUTED:
					interfaceState = INTERFACE_STATE_GET_RESPONSE;
				break;
				default:
					interfaceState = INTERFACE_STATE_AWAITING_EXECUTION;
				break;
			}
		break;
		case INTERFACE_STATE_GET_RESPONSE:
			responseBuffer[responseBufferCounter] = this->spiTransfer(COMMAND_GET_BYTE);
			responseBufferCounter++;

			if (responseBufferCounter >= responseLength) {
				parseResponse();
				interfaceState = INTERFACE_STATE_IDLE;
				commandDone = true;
			}
			else {
				// stay in INTERFACE_STATE_GET_RESPONSE
			}
			
		break;
	}
}

void AOI::recognizeSerialNumber() {
	startCommand(COMMAND_RECOGNIZE_SERIAL_NUMBER);
}
