#include "Arduino.h"
#include "xArm.h"
#include <Controllino.h>

// interface
#define INTERFACE_SETUP_TIME 100	// millis to wait between data output and clock
#define INTERFACE_TIMEOUT (5*1000UL)

enum interfaceStates { 
	INTERFACE_STATE_IDLE,
	INTERFACE_STATE_SEND_BYTE,
	INTERFACE_STATE_SETUP_TIME,
	INTERFACE_STATE_WAITING_FOR_ACK,
	INTERFACE_STATE_WAITING_FOR_ACK_LOW,
	INTERFACE_STATE_WAITING_FOR_EXECUTION,
	INTERFACE_STATE_RESET,
	INTERFACE_STATE_WAITING_FOR_RESET_ACK_LOW
};
interfaceStates interfaceState = INTERFACE_STATE_IDLE;

uint8_t interfaceOutBuffer[3];
uint8_t interfaceIterator = 0;
uint32_t interfaceActionTime = 0;
bool interfaceReturnVal = 0;
bool interfaceCommandSuccess = 1;

#define INTERFACE_CLK_PIN CONTROLLINO_D10
#define INTERFACE_RESET_PIN CONTROLLINO_D9
#define INTERFACE_ACK_PIN CONTROLLINO_A8
#define INTERFACE_COMMAND_OK_PIN CONTROLLINO_A7
#define INTERFACE_EXECUTING_COMMAND_PIN CONTROLLINO_A6
#define INTERFACE_RETURN_PIN CONTROLLINO_A5

// task run and watchdog
#define TASK_START_PIN CONTROLLINO_D11
#define TASK_WATCHDOG_PIN CONTROLLINO_A9
#define TASK_WATCHDOG_TIMEOUT (5*1000UL)
bool taskWatchdogPinState = 0;
uint32_t taskWatchdogChangeTimestamp = 0;

void resetInterface() {
	if (DBG) Serial.println("interface reset");
	digitalWrite(INTERFACE_CLK_PIN, LOW);
	digitalWrite(INTERFACE_RESET_PIN, HIGH);
	interfaceActionTime = millis();
	interfaceState = INTERFACE_STATE_RESET;
}

void interfaceError() {
	if (DBG) Serial.println("interface error");
	interfaceCommandSuccess = 0;
	resetInterface();
}

void interfaceFinished() {
	interfaceReturnVal = digitalRead(INTERFACE_RETURN_PIN);
	interfaceCommandSuccess = 1;
	interfaceState = INTERFACE_STATE_IDLE;
}

void sendNextByte() {
	interfaceIterator++;
	interfaceState = INTERFACE_STATE_SEND_BYTE;
}

void handleInterface() {
	switch (interfaceState) {
		case INTERFACE_STATE_IDLE:
		break;
		case INTERFACE_STATE_SEND_BYTE: {
			// put interfaceIterator byte to output
			uint8_t output = interfaceOutBuffer[interfaceIterator];
			if (DBG) {
				Serial.print("interface send byte: ");
				Serial.println(output);
			}
			for	(uint8_t i = 0; i < 8; i ++) {
				digitalWrite(2+i, output & (1<<i));
			}
			interfaceState = INTERFACE_STATE_SETUP_TIME;
			interfaceActionTime = millis();
		break; }
		case INTERFACE_STATE_SETUP_TIME:
			if ((millis() - interfaceActionTime) >= INTERFACE_SETUP_TIME) {
				digitalWrite(INTERFACE_CLK_PIN, HIGH);
				interfaceState = INTERFACE_STATE_WAITING_FOR_ACK;
				interfaceActionTime = millis();
			}
		break;
		case INTERFACE_STATE_WAITING_FOR_ACK:
			if (digitalRead(INTERFACE_ACK_PIN)) {
				if (DBG) Serial.println("interface ack");
				digitalWrite(INTERFACE_CLK_PIN, LOW);
				interfaceState = INTERFACE_STATE_WAITING_FOR_ACK_LOW;
				interfaceActionTime = millis();
			}
		break;
		case INTERFACE_STATE_WAITING_FOR_ACK_LOW:
			if (!digitalRead(INTERFACE_ACK_PIN)) {
				if (DBG) Serial.println("interface ack low");
				if (interfaceIterator < commandLength[interfaceOutBuffer[0]]-1) sendNextByte();
				else interfaceState = INTERFACE_STATE_WAITING_FOR_EXECUTION;
				interfaceActionTime = millis();
			}
		break;
		case INTERFACE_STATE_WAITING_FOR_EXECUTION:
			if (!digitalRead(INTERFACE_COMMAND_OK_PIN)) {
				if (DBG) Serial.println("interface OK low");
				interfaceError();
				interfaceActionTime = millis();
			}
			else if (!digitalRead(INTERFACE_EXECUTING_COMMAND_PIN)) {
				if (DBG) Serial.println("interface command executed");
				interfaceFinished();
				interfaceActionTime = millis();
			}
		break;
		case INTERFACE_STATE_RESET:
			if (digitalRead(INTERFACE_ACK_PIN)) {
				digitalWrite(INTERFACE_RESET_PIN, LOW);
				interfaceState = INTERFACE_STATE_WAITING_FOR_RESET_ACK_LOW;
				interfaceActionTime = millis();
			}
		break;
		case INTERFACE_STATE_WAITING_FOR_RESET_ACK_LOW:
			if (!digitalRead(INTERFACE_ACK_PIN)) {
				// interface reset finished
				if (DBG) Serial.println("interface reset done");
				interfaceState = INTERFACE_STATE_IDLE;
			}
		break;
	}

	if ((millis() - interfaceActionTime) >= INTERFACE_TIMEOUT) {	// timeout
		if (interfaceState != INTERFACE_STATE_IDLE && interfaceState != INTERFACE_STATE_WAITING_FOR_EXECUTION) resetInterface();
	}
}

void xArm::init() {
	pinMode(TASK_START_PIN, OUTPUT);
	pinMode(INTERFACE_CLK_PIN, OUTPUT);
	pinMode(INTERFACE_RESET_PIN, OUTPUT);
}

void xArm::poll() {
	handleInterface();

	if (digitalRead(TASK_WATCHDOG_PIN) != taskWatchdogPinState) {
		taskWatchdogPinState = digitalRead(TASK_WATCHDOG_PIN);
		taskWatchdogChangeTimestamp = millis();
		if (!taskRunning) {
			taskRunning = true;
			resetInterface();
		}
	}

	if (!taskRunning) {
		digitalWrite(TASK_START_PIN, (millis()/500)%2);	// toggle start pin at 1Hz if task is not running
	}

	if (millis() - taskWatchdogChangeTimestamp >= TASK_WATCHDOG_TIMEOUT) {
		if (taskRunning) {
			//Serial.println("arm task watchdog timeout");
			//taskRunning = false;
		}
	}
}

bool xArm::interfaceAvailable() {
	return (interfaceState == INTERFACE_STATE_IDLE) && taskRunning;
}

bool xArm::sendCommand(uint8_t command, uint8_t param1, uint8_t param2) {
	if (!interfaceAvailable()) return false;
	if (command >= NUM_OF_COMMANDS) return false;
	interfaceOutBuffer[0] = command;
	interfaceOutBuffer[1] = param1;
	interfaceOutBuffer[2] = param2;
	interfaceIterator = 0;
	interfaceState = INTERFACE_STATE_SEND_BYTE;
	interfaceCommandSuccess = 0;
	return true;
}

bool xArm::commandSuccess() {
	return interfaceCommandSuccess;
}

bool xArm::returnVal() {
	return interfaceReturnVal;
}
