#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <assert.h>

#include "local_queue.h"
#include "elev.h"
#include "io.h"
#include "channels.h"
#include "globals.h"
#include "localsys.h"
#include "netmod.h"
#include "mastermod.h"


int dest = -1;
int end_dest = -1;
elev_state_t state;
int current_floor = -1;
char msg[BUFLEN_TCP];


void set_dest(int f) {
	dest = f;
}
int get_dest() {
	return dest;
}

void set_end_dest(int f) {
	end_dest = f;
}

int get_end_dest() {
	return end_dest;
}

int get_state() {
	return state;
}

int get_floor() {
	return current_floor;
}

void status_str(int s, int f, int end) {
	msg[0] = (char) STATUS;
	msg[1] = (char) s;
	msg[2] = (char) f;
	msg[3] = (char) end;
}

int timeDelay(int sec) {
	static int count;
	static time_t start;
	count ++;
	if ( count == 1) {
		time(&start);
	}
	time_t end;
	time(&end);

	if (difftime(end,start) >= sec) {
		count = 0;
		return 1;
	}
	else
		return 0;
}

void* state_machine(){
	while(1) {	  
	   switch (state) {

			case DOOROPEN:
				elev_set_door_open_lamp(1);
				if (io_read_bit(OBSTRUCTION)) break;
				
				if (timeDelay(3)) {
					state = DOORCLOSED;
				    elev_set_door_open_lamp(0);
					break;
				}
				else {
					usleep(1e4);
					break;
				}

			case DOORCLOSED:
				 if (dest < 0){
				 	printf("State: IDLE\n");
					state = IDLE;
					status_str(state, current_floor, end_dest);
					if (get_role() != MASTER) {
						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
					}
					else set_elev_state(-1, msg);
					break;
				 }

				 else if (current_floor < dest){
				    elev_set_speed(150);
				    printf("State: MOVING_UP\n");
				    state = MOVING_UP;
				    status_str(state, current_floor, end_dest);
				    if (get_role() != MASTER) {
						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
					}
					else set_elev_state(-1, msg);
				    break;
				 }

				 else if (current_floor > dest) {
				    elev_set_speed(-150);
				    printf("State: MOVING_DOWN\n");
				    state = MOVING_DOWN;
				    status_str(state, current_floor, end_dest);
				    if (get_role() != MASTER) {
						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
					}
					else set_elev_state(-1, msg);
				    break;
				 }

				 else if (current_floor == dest){
				 	remove_orders();
				 	printf("State: DOOROPEN\n");
					state = DOOROPEN;
					break;
				}	
		
			case IDLE:
				if (dest > -1) {
					state = DOORCLOSED;
					status_str(state, current_floor, end_dest);
					if (get_role() != MASTER) {
						if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
					}
					else set_elev_state(-1, msg);
				}
				usleep(1e4);
			
			default:
				usleep(1e4);
			
		}
		usleep(100);
		
	}
	return NULL;
}

void stop_func(int floor, int value) {
	elev_set_speed(0);
	elev_set_stop_lamp(1);
	state = STOPPED;
	status_str(state, current_floor, end_dest);
	printf("STOPPED\n");
	if (get_role() != MASTER) {
		if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
	}
	else set_elev_state(-1, msg);
	
}

void command_func(int floor, int value) {
	if (state == STOPPED) {
		elev_set_speed(ELEV_GAIN);
		elev_set_stop_lamp(0);
	}
	
	add_to_queue(value, floor);
}

void elev_order_func(int floor, int value) {
	char send[BUFLEN_TCP];
	if (get_role() != MASTER) {
		// Make backup? Check with ping pong etc
		send[0] = (char) ORDER;
		send[1] = (char) value;
		send[2] = (char) floor;
		
		if (tcp_write(get_master_sock(), send) == -1) printf("Cant write to master, do something something...\n");
		
	}
	else {
		int elev = find_opt_elev(value, floor);
		printf("Optimal elevator has socketid %d\n", elev);
		
		if (elev != -1) {
			send[0] = (char) REQUEST;
			if (tcp_write(elev, send) == -1) printf("Failed to give order...\n");
		}
		else add_to_queue(value, floor);
	}
}

void sensor_func(int floor, int value) {
	if (value == 0) return;
	
	elev_set_floor_indicator(floor);
	current_floor = floor;
	printf("Current floor: %d\n", current_floor+1);
	
	if (floor == dest) {
		printf("Arriving at floor %d\n", floor+1);
		elev_set_speed(0);
		remove_orders();
		//Husk å fjerne alle aktuelle ordre ved en funksjon
		state = DOOROPEN;
		status_str(state, current_floor, end_dest);
		if (get_role() != MASTER) {
			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
		}
		else set_elev_state(-1, msg);
		printf("State: DOOROPEN\n");
		return;
	}
	if ((current_floor == N_FLOORS-1) || (current_floor == 0)) {
		printf("Reached end of shaft unexpectedly\n");
		elev_set_speed(0);
		state = DOOROPEN;
		status_str(state, current_floor, end_dest);
		if (get_role() != MASTER) {
			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
		}
		else set_elev_state(-1, msg);
		printf("State: DOOROPEN\n");	
	}
}

void sensor_func_init(int floor, int value) {
	elev_set_speed(0);
	current_floor = floor;
	elev_unregister_callback(SIGNAL_TYPE_SENSOR);
	elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func);
	elev_register_callback(SIGNAL_TYPE_COMMAND, &command_func);
	elev_register_callback(SIGNAL_TYPE_CALL_UP, &elev_order_func);
	elev_register_callback(SIGNAL_TYPE_CALL_DOWN, &elev_order_func);
	elev_register_callback(SIGNAL_TYPE_STOP, &stop_func);
	state = IDLE;
	pthread_t state_th;
	pthread_create(&state_th, NULL, state_machine, NULL);		
	printf("Elevator initialized and ready for use.\n");
	
	status_str(state, current_floor, end_dest);
	if (get_role() != MASTER) {
		if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
	}
	else set_elev_state(-1, msg);
}

int localsys_init() {
	
	if (!elev_init()) {
		printf(__FILE__ ": Unable to initialize elevator hardware\n");
		return 1;
	}

	elev_reset_all_lamps();
	clear_queue();
	
	if (io_read_bit(SENSOR1)) current_floor = 0;
	if (io_read_bit(SENSOR2)) current_floor = 1;
	if (io_read_bit(SENSOR3)) current_floor = 2;
	if (io_read_bit(SENSOR4)) current_floor = 3;
	
	if (current_floor > 0) {
		elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func);
		elev_register_callback(SIGNAL_TYPE_COMMAND, &command_func);
		elev_register_callback(SIGNAL_TYPE_CALL_UP, &elev_order_func);
		elev_register_callback(SIGNAL_TYPE_CALL_DOWN, &elev_order_func);
		elev_register_callback(SIGNAL_TYPE_STOP, &stop_func);
		state = IDLE;
		pthread_t state_th;
		pthread_create(&state_th, NULL, state_machine, NULL);		
		printf("Elevator initialized and ready for use.\n");
		
		status_str(state, current_floor, end_dest);
		if (get_role() != MASTER) {
			if (write(get_master_sock(), msg, BUFLEN_TCP) == -1) printf("Failed to send status\n");
		}
		else set_elev_state(-1, msg);
	}
	else {
		printf("Going to nearest floor...\n");
		elev_register_callback(SIGNAL_TYPE_SENSOR, &sensor_func_init);
		elev_set_speed(-ELEV_GAIN);
	}
	
	int callback_th = elev_enable_callbacks();
	return callback_th;
}