#include <Controllino.h>
#include "PLC.h"
#include "HMI.h"
#include "xArm.h"
#include "SPI.h"
#include "AgingColumn.h"
#include "AOI.h"

#define COL_0_SS_PIN CONTROLLINO_PIN_HEADER_DIGITAL_OUT_15
#define COL_1_SS_PIN CONTROLLINO_UART_RX
#define AOI_SS_PIN CONTROLLINO_PIN_HEADER_DIGITAL_OUT_13

xArm arm = xArm();
AgingColumn column0 = AgingColumn();
AgingColumn column1 = AgingColumn();
AOI aoi = AOI();
HMI hmi = HMI(&arm, &column0, &column1, &aoi);

agingStates agingState = AGING_STATE_IDLE;
agingSubstates agingSubstate = AGING_SUBSTATE_FILLING;
nixieTypes nixieType = NIXIE_R;

agingStates agingStateBeforeError = AGING_STATE_IDLE;

void startAging(int8_t substateType) {
	agingState = AGING_STATE_RUNNING;
	if (substateType == -1) {	// auto mode
		agingSubstate = AGING_SUBSTATE_FILLING;
	}
	else {	// manual mode
		agingSubstate = substateType;
	}
}

void stopAging() {
	agingState = AGING_STATE_IDLE;
}

void pauseAging() {
	agingState = AGING_STATE_PAUSED;
}

void resumeAging() {
	agingState = AGING_STATE_RUNNING;
}

void changeNixieType(uint8_t type) {
	nixieType = type;
}

void agerStateMachine() {
	if (!arm.taskRunning) {		// communication error
		if (agingState != AGING_STATE_ERROR) agingState = AGING_STATE_ERROR;
	}
	else {
		if (agingState == AGING_STATE_ERROR) agingState = agingStateBeforeError;
	}

	switch (agingState) {
		case AGING_STATE_IDLE:
			agingStateBeforeError = AGING_STATE_IDLE;
		break;
		case AGING_STATE_PAUSED:
			agingStateBeforeError = AGING_STATE_PAUSED;
		break;
		case AGING_STATE_RUNNING:
			agingStateBeforeError = AGING_STATE_RUNNING;
			switch (agingSubstate) {
				case AGING_SUBSTATE_FILLING:
				break;
				case AGING_SUBSTATE_EMPTYING:
				break;
				case AGING_SUBSTATE_CYCLE_1:
				break;
				case AGING_SUBSTATE_CYCLE_2:
				break;
				case AGING_SUBSTATE_HG_RELEASE:
				break;
			}
		break;
		case AGING_STATE_ERROR:
			// fatal error, cannot continue
		break;
	}
}

void setup() {
	Serial.begin(9600);
	Serial.println("Ager 3.0 PLC starting");
	hmi.init();
	arm.init();
	column0.init(COL_0_SS_PIN);
	column1.init(COL_1_SS_PIN);
	aoi.init(AOI_SS_PIN);
}

uint8_t subposition = 0;

void loop(void) {
	hmi.poll();
	arm.poll();
	aoi.poll();
	agerStateMachine();

	if (arm.interfaceAvailable()) {
		if (subposition == 0) arm.sendCommand(COMMAND_MOVE, 0, 0);
		if (subposition == 1) arm.sendCommand(COMMAND_PUT_DOWN);
		if (subposition == 2) arm.sendCommand(COMMAND_PICK_UP);
		if (subposition == 3) arm.sendCommand(COMMAND_MOVE, 1, 0);
		if (subposition == 4) arm.sendCommand(COMMAND_PUT_DOWN);
		if (subposition == 5) arm.sendCommand(COMMAND_PICK_UP);
		subposition++;
		if (subposition >= 6) subposition = 0;
	}
}
