#include "door.h"
#include "log.h"

uint8_t door_read_pin_status(void);
uint8_t door_update_status(uint8_t sensor_status);
uint8_t is_alarm_status(uint8_t door_status);

char status_unlocked[]  PROGMEM = "Unlocked";
char status_locked[]    PROGMEM = "Locked";
char status_open[]      PROGMEM = "Open";
char status_alarm_cut[] PROGMEM = "Alarm wire open";
char status_alarm[]     PROGMEM = "Alarm";
char status_unlocking[] PROGMEM = "Unlocking...";
char status_locking[]   PROGMEM = "Locking...";
char* status_names[] PROGMEM = {status_unlocked,
                                status_locked,
                                status_open,
                                status_alarm_cut,
                                status_alarm,
                                status_unlocking,
                                status_locking};

volatile uint8_t door_status;

volatile uint8_t sensor_candidate_status;
volatile uint8_t sensor_stability;
#define DOOR_PIN_NEEDED_STABILITY 4

#define DOOR_COMMAND_PULSE_TIME 35 //350ms
volatile uint8_t door_command_pulse_time = 0;

#define DOOR_COMMAND_WAIT_TIMEOUT 1000 //10s
volatile uint16_t door_command_wait_timeout = 0;

void door_init(void){
	DOOR_PORT &= ~(COMMAND_UNLOCK_PIN | COMMAND_LOCK_PIN); //commnd pins off
	DOOR_DDR  |= COMMAND_UNLOCK_PIN | COMMAND_LOCK_PIN;    //and output
	
	DOOR_DDR  &= ~DOOR_PIN_MASK; //sensor pins to input
	DOOR_PORT |= DOOR_PIN_MASK; //and pullup on
	
	PCMSK2 = DOOR_PIN_MASK; //enable pin change interrupts on DOOR STATUS pins
	PCICR = (1 << PCIE2); //enable pin change interrupt on PORTC (PCINT block 2)
	
	KEYMATIC_POWER_DDR |= KEYMATIC_POWER_LINE;
	
	door_status = door_read_pin_status();
	sensor_candidate_status = door_status;
	sensor_stability = DOOR_PIN_NEEDED_STABILITY;
}

void door_tick(void){
	if (door_command_pulse_time != 0) {
		door_command_pulse_time--;
		if (door_command_pulse_time == 0) DOOR_PORT &= (uint8_t)~(COMMAND_LOCK_PIN|COMMAND_UNLOCK_PIN);
	}
	
	if (   ((door_status & DOOR_STATUS_UNLOCKING) && !(door_status & DOOR_STATUS_UNLOCKED))
		|| ((door_status & DOOR_STATUS_LOCKING)   && !(door_status & DOOR_STATUS_LOCKED))   ){
		door_command_wait_timeout--;
		if (door_command_wait_timeout == 0){
			
			printf_P(PSTR("Error: Door command timeout\n"));
			log_append(LOG_EVENT_DOOR_COMMAND_TIMEOUT, 0);
			
			//TODO: Retry (and turn keymatic power on if currently off)
			
			door_status &= (uint8_t)~(DOOR_STATUS_UNLOCKING | DOOR_STATUS_LOCKING); //clear activity flags
		}
	}
	
	uint8_t door_current_pin_status = door_read_pin_status();

	if (door_current_pin_status == sensor_candidate_status){
		if (sensor_stability == DOOR_PIN_NEEDED_STABILITY){
			//Sensor pins are stable update the HW status bits in the status var
			door_status = door_update_status(sensor_candidate_status);
		}
		if (sensor_stability <= DOOR_PIN_NEEDED_STABILITY) sensor_stability++;
		
	} else {
		sensor_candidate_status = door_current_pin_status;
		sensor_stability = 0;
	}
	
}

uint8_t door_read_pin_status(void){
	uint8_t status_pins;
	status_pins = DOOR_PIN & DOOR_PIN_MASK;
	status_pins ^= DOOR_PIN_FLIP; //flip bits for active low pins so that "bit set" always means active.
	return (status_pins >> DOOR_PIN_OFFSET);
}

uint8_t door_update_status(uint8_t sensor_status){
	uint8_t new_door_status = (sensor_status & DOOR_STATUS_HW_MASK) | (door_status & DOOR_STATUS_SW_MASK);

	if (new_door_status & DOOR_STATUS_LOCKED)   new_door_status &= (uint8_t)~DOOR_STATUS_LOCKING;
	if (new_door_status & DOOR_STATUS_UNLOCKED) new_door_status &= (uint8_t)~DOOR_STATUS_UNLOCKING;
	
	if (is_alarm_status(new_door_status)) new_door_status |= DOOR_STATUS_ALARM;
	
	if (new_door_status & DOOR_STATUS_ALARM){
		log_append( (door_status & DOOR_STATUS_ALARM) ? LOG_EVENT_ALARM_CHANGED : LOG_EVENT_ALARM_RAISED, 0);
	}
	
	return new_door_status;
}

uint8_t is_alarm_status(uint8_t door_status){
		
	if (door_status & DOOR_STATUS_ALARM_CUT)
		return 1;
	
	//NOTE: This condition is supposed to trigger, if the door is slammed open.
	//      We can't use the LOCKED sensor, because the connection between lock bolt and frame will be interrupted by this.
	//      This connection is what the sensor actually detects. TODO: Can we improove this?
	//      The UNLOCKED sensor, however will be uneffected, leaving the lock sensors in an
	//      undefined (neither locked nor unlocked) state. We trigger an alarm if this occurs when the door is open.
	//      Unfortunately the UNLOCKED has shown failures in the past, possibly causing a false alarm.
	if ((door_status & DOOR_STATUS_OPEN) && !(door_status & DOOR_STATUS_UNLOCKED))
		return 1;
	
	//An undefined state (neither locked nor unlocked) may only occur if we have issued a LOCKING or UNLOCKING command
	if ((door_status & (DOOR_STATUS_LOCKED | DOOR_STATUS_UNLOCKED | DOOR_STATUS_UNLOCKING | DOOR_STATUS_LOCKING)) == 0)
		return 1;
		
	return 0;
}

uint8_t door_get_status(void){
	return door_status;
}


void unlock(void){
	door_command_pulse_time = DOOR_COMMAND_PULSE_TIME;
	door_command_wait_timeout = DOOR_COMMAND_WAIT_TIMEOUT;
	
	door_status = (door_status & (uint8_t)~DOOR_STATUS_LOCKING) | DOOR_STATUS_UNLOCKING;
	DOOR_PORT   = (DOOR_PORT   & (uint8_t)~COMMAND_LOCK_PIN)    | COMMAND_UNLOCK_PIN;
}

void lock(void){
	door_command_pulse_time = DOOR_COMMAND_PULSE_TIME;
	door_command_wait_timeout = DOOR_COMMAND_WAIT_TIMEOUT;
	
	door_status = (door_status & (uint8_t)~DOOR_STATUS_UNLOCKING) | DOOR_STATUS_LOCKING;
	DOOR_PORT   = (DOOR_PORT   & (uint8_t)~COMMAND_UNLOCK_PIN)    | COMMAND_LOCK_PIN;
}

uint8_t toggle_lock_unlock(void){
	if ((door_status & DOOR_STATUS_UNLOCKED) == 0){
// 	if (door_status & DOOR_STATUS_LOCKED){
		return unlock_checked();
	} else {
		return lock_checked();
	}
}

uint8_t unlock_checked(void){
	if (door_status & DOOR_STATUS_UNLOCKED){
		printf_P(PSTR("Already unlocked\n"));
	} else if (door_status & DOOR_STATUS_LOCKING){
		printf_P(PSTR("Can't unlock while locking in progress\n"));
	} else {
		unlock();
		return 1;
	}
	return 0;
}

uint8_t lock_checked(void){
	if (door_status & DOOR_STATUS_ALARM){
		printf_P(PSTR("Can't lock in alarm state\n"));
	} else if (door_status & DOOR_STATUS_UNLOCKING) {
		printf_P(PSTR("Can't lock while unlocking in progress\n"));
	} else if (door_status & DOOR_STATUS_OPEN){
		printf_P(PSTR("Can't lock while door open\n"));
	} else if (door_status & DOOR_STATUS_LOCKED){
		printf_P(PSTR("Already locked\n"));
	} else {
		lock();
		return 1;
	}
	return 0;
}

uint8_t door_clear_alarm(void){
	if (is_alarm_status(door_status)) return 0;
	
	log_append(LOG_EVENT_ALARM_DISABLED, 0);
	
	door_status &= (uint8_t)~DOOR_STATUS_ALARM;
	return 1;
}

void door_set_keymatic_charge_power(uint8_t on){
	printf_P(PSTR("Keymatic charge voltage turned "), on);
	if (on) {
		printf_P(PSTR("on\n"));
		KEYMATIC_POWER_PORT |= KEYMATIC_POWER_LINE;
	} else {
		printf_P(PSTR("off\n"));
		KEYMATIC_POWER_PORT &= ~KEYMATIC_POWER_LINE;
	}
}

void door_write_status(void){
	uint8_t status = door_status; //copy to avoid race with interrupt
	printf_P(PSTR("Status (%02X): "), status);
	uint8_t c = 0;
	for (uint8_t i = 0; i <= DOOR_MAX_STATUS_FLAG; i++){
		if (status & (1 << i)){
			if (c) printf_P(PSTR(","));
			c++;
			printf_P((PGM_P)pgm_read_word(&(status_names[i])));
		}
	}
	
	if (status == 0) printf_P(PSTR("Unknown"));
// 	printf_P(PSTR(" "));
}