#include <rtai_mbx.h>
#include <rtai_sched.h>
#include <rtai_sem.h>
#include <sys/rtai_modbus.h>

//======================
//Author: Charlotte Friedemann, Johannes Theiner
//Description: Praktikum EZDV Gruppe A5(Bearbeiten 2)
//Created: 19.10.2020
//Finished: 30.11.2020
//======================

#define SENSOR_PART_TURNTABLE 1<<0
#define SENSOR_PART_DRILL 1<<1
#define SENSOR_PART_TESTER 1<<2
#define SENSOR_DRILL_UP 1<<3
#define SENSOR_DRILL_DOWN 1<<4
#define SENSOR_TURNTABLE_POS 1<<5
#define SENSOR_PART_TEST 1<<6

#define ACTOR_DRILL 1<<0
#define ACTOR_TURNTABLE 1<<1
#define ACTOR_DRILL_DOWN 1<<2
#define ACTOR_DRILL_UP 1<<3
#define ACTOR_PART_HOLD 1<<4
#define ACTOR_TESTER 1<<5
#define ACTOR_EXIT 1<<6
#define ACTOR_ENTRANCE 1<<7

MODULE_LICENSE("GPL");

static RT_TASK turntable_task, drill_task, tester_task, output_task;
static MBX turntable_status_mbx, tester_status_mbx;
static MBX output_status_mbx, drill_status_mbx;
static MBX output_data_mbx, drill_data_mbx, turntable_data_mbx;
static SEM semaphore;

int connection;

char node[] = "modbus-node";

/**
 * deinitialize all tasks, mailboxes, modbus connections & semaphores
 * @param fail should a error message be printed.
 */
void end(bool fail) {
    rt_modbus_disconnect(connection);
    rt_task_delete(&turntable_task);
    rt_task_delete(&tester_task);
    rt_task_delete(&drill_task);
    rt_task_delete(&output_task);

    rt_mbx_delete(&turntable_status_mbx);
    rt_mbx_delete(&tester_status_mbx);
    rt_mbx_delete(&drill_status_mbx);
    rt_mbx_delete(&output_status_mbx);
    rt_mbx_delete(&turntable_data_mbx);
    rt_mbx_delete(&drill_data_mbx);
    rt_mbx_delete(&output_data_mbx);

    rt_sem_delete(&semaphore);

    stop_rt_timer();

    if(fail) {
        rt_printk("an error has occurred.\n");
        rt_printk("module needs to be restarted.\n");
    }
}

/**
 * read all bits of the specified type
 * this function is *not* thread safe.
 * @param type (DIGITAL_IN, DIGITAL_OUT, ANALOG_IN, ANALOG_OUT)
 * @param result value to write the result to
 * @return status code(0: success, 1: failure)
 */
int readAll(int type, int *result) {
    int res = rt_modbus_get(connection, type, 0,
                            (unsigned short *) result);
    return res;
}

/**
 * read a single bit for the specified part
 * this function is thread safe.
 * @param part bitmask to read the sensor/actor
 * @return read bit.
 */
int readData(int part) {
    int value = 0;
	rt_sem_wait(&semaphore);
	//the semaphore needs to be locked before this code executes,
	// so we can't adhere to the ISO standard here.
    int code = readAll(DIGITAL_IN, &value);
    int result = (part & value);
    rt_sem_signal(&semaphore);
    if(code) end(true);
    return result;
}

/**
 * disable a specified actor of the system.
 * this function is thread safe.
 * @param actor bitmask of the actor to disable
 */
void disable(int actor) {
	int value = 0;
    rt_sem_wait(&semaphore);
    if(readAll(DIGITAL_OUT, &value)) end(true);
    else {
    	int result = rt_modbus_set(connection, DIGITAL_OUT, 0,
                                value &= ~actor);
    	rt_sem_signal(&semaphore);
    	if (result) end(true);
    }
}

/**
 * enable one specified actor of the system.
 * this function is thread safe.
 * @param actor bitmask of the actor to enable
 */
void enable(int actor) {
	int output = 0;
    rt_sem_wait(&semaphore);
    if (readAll(DIGITAL_OUT, &output)) end(true);
    else {
    	int result = rt_modbus_set(connection, DIGITAL_OUT, 0,
                                output |= actor);
    	rt_sem_signal(&semaphore);
    	if(result) end(true);
    }

}

/**
 * send a blocking message to the specified mailbox
 * @param mailbox where should the message be send to ?
 * @param msg message to send
 */
void sendMail(MBX * mailbox, int msg) {
    int mbxStatus = rt_mbx_send(mailbox, &msg, sizeof(int));
    if(mbxStatus == EINVAL) end(true);
}

/**
 * send a non blocking message to the specified mailbox
 * @param mailbox where should the message be send to ?
 * @param msg message to send
 */
void sendMailNonBlocking(MBX * mailbox, int msg) {
    int mbxStatus = rt_mbx_send_if(mailbox, &msg, sizeof(int));
    if(mbxStatus == EINVAL) end(true);
}

/**
 * receive a blocking message
 * @param mailbox mailbox to receive the message
 */
int receiveMail(MBX * mailbox) {
    int msg = 0;
    int mbxStatus = rt_mbx_receive(mailbox, &msg, sizeof(int));
    if(mbxStatus == EINVAL) end(true);
    return msg;
}

/**
 * receive a non blocking message
 * @param mailbox mailbox to receive the message
 */
int receiveMailNonBlocking(MBX * mailbox) {
    int msg = 0;
    int mbxStatus = rt_mbx_receive_if(mailbox, &msg, sizeof(int));
    if(mbxStatus == EINVAL) end(true);
    return msg;
}

/**
 * sleep for x milliseconds
 * @param ms specified time to sleep in milliseconds.
 */
void sleepMs(int ms) {
    rt_sleep(ms * nano2count(1000000));
}

/**
 * turntable task
 */
static void turntable(long data) {
	int times = 0;
	rt_printk("started turntable task\n");

	//reset everything first
    disable(ACTOR_TURNTABLE);
    disable(ACTOR_ENTRANCE);

    //throw out all parts that might be on the table.
    do {
    	enable(ACTOR_TURNTABLE);
    	sleepMs(1000);
    	disable(ACTOR_TURNTABLE);
    	sleepMs(500);
    	enable(ACTOR_EXIT);
    	sleepMs(500);
    	disable(ACTOR_EXIT);
    	times++;
    }while (times < 5);

    //start processing
    while (1) {
    	//receive status mail from: tester, drill & output
        receiveMail(&turntable_status_mbx);
        receiveMail(&turntable_status_mbx);
        receiveMail(&turntable_status_mbx);
        //always read from mailbox to make sure that no overflow occurs.
        int msg1 = receiveMailNonBlocking(&turntable_data_mbx);
        int msg2 = receiveMailNonBlocking(&turntable_data_mbx);
        //if a part is on any of the sensors
        if(readData(SENSOR_PART_TURNTABLE) || msg1 || msg2) {
            enable(ACTOR_TURNTABLE);
            sleepMs(100);
			if (!readData(SENSOR_TURNTABLE_POS)) {
				do {
					sleepMs(10);
				} while (readData(SENSOR_TURNTABLE_POS) == 0);
			}
        	disable(ACTOR_TURNTABLE);
            sleepMs(500);
        }
        //send status mails
        sendMail(&tester_status_mbx, 1);
        sendMail(&drill_status_mbx, 1);
        sendMail(&output_status_mbx, 1);
    }
}

/**
 * tester task
 */
static void tester(long data) {
    rt_printk("started tester task\n");

    //reset everything first
	disable(ACTOR_TESTER);
	sendMail(&turntable_status_mbx, 1);
	//start processing
    while (1) {
        receiveMail(&tester_status_mbx);
    	if(readData(SENSOR_PART_TESTER)) {
            //turntable should turn on the next turn.
    		sendMailNonBlocking(&turntable_data_mbx, 1);
    		enable(ACTOR_TESTER);
            sleepMs(500);
            //should the drill be active on the next turn ?
    		if(readData(SENSOR_PART_TEST)) {
    			sendMailNonBlocking(&drill_data_mbx, 1);
    		}else {
    			sendMailNonBlocking(&drill_data_mbx, 0);
    		}
    		disable(ACTOR_TESTER);
    	}
    	sendMail(&turntable_status_mbx, 1);
    }
}

/**
 * drill task
 */
static void drill(long data) {
	rt_printk("started drill task\n");
	//reset everything first
    disable(ACTOR_DRILL);
    disable(ACTOR_PART_HOLD);
    disable(ACTOR_DRILL_DOWN);

    enable(ACTOR_DRILL_UP);
    if (!readData(SENSOR_DRILL_UP)) {
        do {
        	sleepMs(10);
        } while (readData(SENSOR_DRILL_UP) == 0);
    }
    disable(ACTOR_DRILL_UP);
    sendMail(&turntable_status_mbx, 1);
    //start processing
    while (1) {
        receiveMail(&drill_status_mbx);
        //if part is in drill
    	if(readData(SENSOR_PART_DRILL)) {
            //turntable should be active on the next turn.
    		sendMailNonBlocking(&turntable_data_mbx, 1);
            //read test result from mailbox.
    		if(receiveMailNonBlocking(&drill_data_mbx)) {
    			enable(ACTOR_PART_HOLD);
    			enable(ACTOR_DRILL);
    			enable(ACTOR_DRILL_DOWN);
    			if (!readData(SENSOR_DRILL_DOWN)) {
    				do {
    					sleepMs(10);
    			    } while (readData(SENSOR_DRILL_DOWN) == 0);
    			}
                sleepMs(1000);
				disable(ACTOR_DRILL);
				disable(ACTOR_DRILL_DOWN);
				enable(ACTOR_DRILL_UP);
				if (!readData(SENSOR_DRILL_UP)) {
					do {
						sleepMs(10);
					} while (readData(SENSOR_DRILL_UP) == 0);
				}
				disable(ACTOR_DRILL_UP);
				disable(ACTOR_PART_HOLD);
    		}
            //should the output be active on the next turn ?
    		sendMailNonBlocking(&output_data_mbx, 1);
    	}
    	sendMail(&turntable_status_mbx, 1);
    }
}

/**
 * output task
 */
static void output(long data) {
	rt_printk("started output task\n");
	//reset everything first
    disable(ACTOR_EXIT);
    sendMail(&turntable_status_mbx, 1);
    //start processing
    while (1) {
        receiveMail(&output_status_mbx);
        //should the output be activated
    	if(receiveMailNonBlocking(&output_data_mbx)) {
    		enable(ACTOR_EXIT);
            sleepMs(500);
    		disable(ACTOR_EXIT);
    	}
    	sendMail(&turntable_status_mbx, 1);
    }
}

static int __init
example_init(void) {
    rt_set_oneshot_mode();
    start_rt_timer(0);
    modbus_init();

    rt_printk("init: started\n");
    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("init: could not connect to %s\n", node);
        return -1;
    }

    rt_sem_init(&semaphore, 1);

    if (rt_task_init(&turntable_task, turntable, 0, 1024, 0, 0, NULL)) {
        rt_printk("turntable: cannot initialize task\n");
        end(true);
    }
    if (rt_task_init(&drill_task, drill, 0, 1024, 0, 0, NULL)) {
       rt_printk("drill: cannot initialize task\n");
        end(true);
    }
    if (rt_task_init(&output_task, output, 0, 1024, 0, 0, NULL)) {
        rt_printk("output: cannot initialize task\n");
         end(true);
    }
    if (rt_task_init(&tester_task, tester, 0, 1024, 0, 0, NULL)) {
        rt_printk("tester: cannot initialize task\n");
        end(true);
    }

    if (rt_mbx_init(&turntable_status_mbx, sizeof(int))) {
    	end(true);
    }
    if (rt_mbx_init(&tester_status_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&drill_status_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&output_status_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&output_data_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&drill_data_mbx, sizeof(int))) {
    	end(true);
    }
    if (rt_mbx_init(&turntable_data_mbx, sizeof(int))) {
    	end(true);
    }

    rt_task_resume(&turntable_task);
    rt_task_resume(&drill_task);
    rt_task_resume(&output_task);
    rt_task_resume(&tester_task);
    rt_printk("loaded module Bearbeiten2\n");
    return (0);
}

static void __exit
example_exit(void) {
    end(false);
    rt_printk("module Bearbeiten2 unloaded\n");
}

module_exit(example_exit)
module_init(example_init)