#include <conio.h>
#include <string.h>
#include <time.h>
#include <semaphore.h>
#include <lnp/lnp-logical.h>
#include <lnp/lnp.h>

#include "com.h"


//=====================================================================

sem_t sem_com;

/* declare & initialize TRIGGER1 */
int TRIGGER1 = 0;
/* declare & initialize TRIGGER2 */
int TRIGGER2 = 0;
/* declare & initialize TRIGGER3 */
int TRIGGER3 = 0;
/* declare & initialize TRIGGER4 */
int TRIGGER4 = 0;
/* declare & initialize TRIGGER5 */
int TRIGGER5 = 0;
/* declare & initialize TEMPERATURE */
int TEMPERATURE = 0;
/* declare & initialize COORDINATES */
int COORDINATES[9] = {0,0,0,0,0,0,0,0,0};


//=====================================================================


/* declare thread id for read_from_ir_thread */
tid_t read_from_ir_thread;

int COM_ID=0;

/* declare received_message */
char received_message[40];
/* declare receiver_header */
char received_header[3];
/* declare & initialize received_message_len */
int received_message_len = 0;

//=====================================================================



/****************************************/
/* begin own_atoi: convert s to integer */
/****************************************/

int own_atoi (char s[]) {
    int i, n, sign;
    
    for (i=0; s[i] == ' ' || s[i] == '\n' || s[i] == '\t' ; i++) {
    }
    sign = (s[i] == '-') ? -1 : 1;
    if (s[i] == '+' || s[i] == '-')
        i++;
    for (n=0; s[i] >= '0' && s[i] <= '9'; i++)
        n = 10*n + (s[i] - '0');
    return sign * n;
}

/****************************************/
/* end own_atoi: convert s to integer   */
/****************************************/

/***************************************/
/* begin own_reverse: reverse string s */
/***************************************/

void own_reverse (char s[]) {
    int i, j;
    char c;
    
    for (i=0, j = strlen(s) -1; i<j; i++, j--) {
        c = s[i];
        s[i] = s[j];
        s[j] = c;
    }
}
/***************************************/
/* end own_reverse: reverse string s   */
/***************************************/

/*****************************************/
/* begin own_itoa: convert n to string s */
/*****************************************/

void own_itoa (int n, char s[]) {
    int i, sign;
    
    if ((sign = n) < 0)
        n=-n;
    i=0;
    do {
        s[i++] =  (char) ( n % 10 + (int) '0');
    } while ((n /= 10) > 0);
    if (sign<0)
        s[i++]='-';
    s[i] = '\0';
    own_reverse(s);
}
/*****************************************/
/* end own_itoa: convert n to string s   */
/*****************************************/

/******************************************/
/* begin porthandler: define port handler */
/******************************************/

static void port_handler(const unsigned char *data, unsigned char len)
{
    /* declare i */
    int i;
    
    /* is the received packet for RCX ID? */
    if (data[1]==COM_ID+'0'){
        
        for(i=0;i<=len;i++) {
            if (i<3) {
                /* received header */
                received_header[i]=data[i];
            } else {
                /* received message */
                received_message[i-3]=data[i];
            }
                
        }
        /* received message length */
        received_message_len = len-3;
    }
}

/***************************************/
/* end porthandler: set port handler   */
/***************************************/

/***************************************/
/* begin havepack: wait function       */
/***************************************/

static wakeup_t havepack(wakeup_t w)
{
	return received_message_len;
}

/***************************************/
/* end havepack: wait function         */
/***************************************/

/***************************************/
/* begin read_from_ir: read from ir    */
/***************************************/

int read_from_ir(int argc, char *argv[])
{
    /* declare i, j and k */
    int i, j, k;
    
    /* declare data_header */
    char data_header[4];
    /* declare data_received */    
    char data_received[40];
    /* declare data_header */
    char number_string[5];
    
    int data_received_int[9];
    
    while(!shutdown_requested()) {
        
        /* wait for havepack */
        wait_event(havepack, 0);
        
        /* copy received_message to data_received */
        memcpy(data_received, received_message, received_message_len);
        /* copy received_header to data_header */
        memcpy(data_header, received_header, 3);
        
        /* initialize k and j */
        j = 0;
        k = 0;
        
        for (i=0;i<received_message_len;i++) {
            if (data_received[i]!=' ' && data_received[i] != '\0') {
                number_string[j] = data_received[i];
                j++;
                
            } else if (data_received[i] == ' ' || data_received[i] == '\0') {
                number_string[j] = '\0';
                /* convert string number_string to integer */
                data_received_int[k] = own_atoi(number_string);
                k++;
                j = 0;
            }
        }
        
        /* wait for semaphore key */
        sem_wait(&sem_com);
        
        /* write in global variables */
        switch (data_header[2]) {
            case '1':
                TRIGGER1=data_received_int[0];
                break;
            case '2':
                TRIGGER2=data_received_int[0];
                break;
            case '3':
                TRIGGER3=data_received_int[0];
                break;
            case '4':
                TRIGGER4=data_received_int[0];
                break;
            case '5':
                TRIGGER5=data_received_int[0];
                break;
            case '6':
                TEMPERATURE=data_received_int[0];
                break;
            case '7':
                for (i=0;i<9;i++) {
                    COORDINATES[i]=data_received_int[i];
                }
                break;
        }

        /* wait for semaphore key */
        sem_post(&sem_com);

        received_message_len = 0;
    }
    
    return 0;
}
/*************************************/
/* end read_from_ir: read from ir    */
/*************************************/

/***************************************************************************/
/* begin com_send: send message with LNP from RCX to RCX or from RCX to PC */
/***************************************************************************/

int com_send(int id_sender_int, int id_receiver_int,
             int message_type_int, int *message_send_int) {

    /* declare i, j and k */    
    unsigned int i,j,k;
    /* declare id_sender_string */
    char id_sender_string[2];
    /* declare id_receiver_string */
    char id_receiver_string[2];
    /* declare message_type_string */
    char message_type_string[2];
    /* declare message_send_string */
    char message_send_string[4];
    /* declare message_send_string */
    char message_send[40];
        
    int message_len;

    /* create header */

    /* convert integer id_sender_int to string */
    own_itoa(id_sender_int,id_sender_string);
    message_send[0]=id_sender_string[0];
    /* convert integer id_receiver_int to string */
    own_itoa(id_receiver_int,id_receiver_string);
    message_send[1]=id_receiver_string[0];
    /* convert integer message_type_int to string */
    own_itoa(message_type_int,message_type_string);
    message_send[2]=message_type_string[0];
    

    /* create message body */

    /* initialize k */
    k = 2;
    
    if ((message_type_int>=1) && (message_type_int<=6)) {
        /* convert integer message_send_int[0] to string */        
        own_itoa(message_send_int[0],message_send_string);
        
        for (j=0;j<strlen(message_send_string);++j) {
            k++;
            message_send[k]=message_send_string[j];
        }
        
    } else if (message_type_int == 7) {
        for (i=0;i<9;++i) {
            /* convert integer message_send_int[i] to string */
            own_itoa(message_send_int[i],message_send_string);
            
            for (j=0;j<strlen(message_send_string);++j) {
                k++;
                message_send[k]=message_send_string[j];
            }
            if (i!=9-1) {
                k++;
                message_send[k]=' ';
            }
        }
    } else {
        message_send[3]='\0';
    }
    
    k++;
    message_send[k]='\0';
    

    message_len =strlen(message_send)+1;
    /* send message */
    lnp_integrity_write((unsigned char *)message_send,message_len);


    return 0;
}

/*************************************************************************/
/* end com_send: send message with LNP from RCX to RCX or from RCX to PC */
/*************************************************************************/


int init_com(int id) {

    COM_ID=id;
 
    /* initialize communication port */
    lnp_integrity_set_handler(port_handler);
    /* set ir range to "far" */
    lnp_logical_range(1);
    /* initialize semaphore for communication */
    sem_init(&sem_com, 0, 1);


    /* start read_from_ir thread */
	read_from_ir_thread = execi(&read_from_ir,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);

    return 0;
}