#include "recognize.h"

// Match and return the command
int matchCommand(int length, int peak, int valley, int landingSwitch){
	int up = 0, down = 0, idle = 0, land = 0;

	if (landingSwitch){
		switch(checkLength(length, landingSwitch)){
		case STATUS_IDLE:
			idle++;
			break;
		case STATUS_LAND:
			land++;
			break;
		default:
			;
		};

		switch(checkPeak(peak, landingSwitch)){
		case STATUS_IDLE:
			idle++;
			break;
		case STATUS_LAND:
			land++;
			break;
		default:
			;
		};

		switch(checkValley(valley, landingSwitch)){
		case STATUS_IDLE:
			idle++;
			break;
		case STATUS_LAND:
			land++;
			break;
		default:
			;
		};
	}
	else{
		switch(checkLength(length, landingSwitch)){
		case STATUS_UP:
			up++;
			break;
		case STATUS_DOWN:
			down++;
			break;
		default:
			;
		};

		switch(checkPeak(peak, landingSwitch)){
		case STATUS_UP:
			up++;
			break;
		case STATUS_DOWN:
			down++;
			break;
		default:
			;
		};

		switch(checkValley(valley, landingSwitch)){
		case STATUS_UP:
			up++;
			break;
		case STATUS_DOWN:
			down++;
			break;
		default:
			;
		};
	}

	if(up >= 2){
		return STATUS_UP;
	}
	else if (down >= 2){
		return STATUS_DOWN;
	}
	else if (idle >= 2){
		return STATUS_IDLE;
	}
	else if (land >= 2){
		return STATUS_LAND;
	}
	else {
		return -1;
	}
}

// Match the length of the command
int checkLength(int length, int landingSwitch){
	if (landingSwitch){
		if (length >= IDLE_LENGTH_LOWER && length < IDLE_LENGTH_UPPER){
			return STATUS_IDLE;
		}
		else if (length >= LAND_LENGTH_LOWER && length < LAND_LENGTH_UPPER){
			return STATUS_LAND;
		}
	}
	else {
		if (length >= UP_LENGTH_LOWER && length < UP_LENGTH_UPPER){
			return STATUS_UP;
		}
		else if(length >= DOWN_LENGTH_LOWER && length < DOWN_LENGTH_UPPER){
			return STATUS_DOWN;
		}
	}
	return -1;
}

// Match the positive maximum value of the command
int checkPeak(int peak, int landingSwitch){
	if (landingSwitch){
		if (peak >= IDLE_PEAK_LOWER && peak < IDLE_PEAK_UPPER){
			return STATUS_IDLE;
		}
		if (peak >= LAND_PEAK_LOWER && peak < LAND_PEAK_UPPER){
			return STATUS_LAND;
		}
	}
	else {
		if (peak >= UP_PEAK_LOWER && peak < UP_PEAK_UPPER){
			return STATUS_UP;
		}
		else if (peak >= DOWN_PEAK_LOWER && peak < DOWN_PEAK_UPPER){
			return STATUS_DOWN;
		}
	}
	return -1;
}

// Match the negative maximum value of the command
int checkValley(int valley, int landingSwitch){
	if (landingSwitch){
		if (valley >= IDLE_VALLEY_LOWER && valley < IDLE_VALLEY_UPPER){
			return STATUS_IDLE;
		}
		if (valley >= LAND_VALLEY_LOWER && valley < LAND_VALLEY_UPPER){
			return STATUS_LAND;
		}
	}
	else {
		if (valley >= UP_VALLEY_LOWER && valley < UP_VALLEY_UPPER){
			return STATUS_UP;
		}
		else if (valley >= DOWN_VALLEY_LOWER && valley < DOWN_VALLEY_UPPER){
			return STATUS_DOWN;
		}
	}
	return -1;
}
