/******************************************************************************
* Copyright (c) 2006 Altera Corporation, San Jose, California, USA.           *
* All rights reserved. All use of this software and documentation is          *
* subject to the License Agreement located at the end of this file below.     *
*******************************************************************************                                                                           *
* Date - October 24, 2006                                                     *
* Module - led.c                                                              *
*                                                                             *
******************************************************************************/


/* 
 * Unimation client/Controller. 
 * 
 * Please refer to the Altera NicheStack Tutorial documentation for details on this 
 * software example, as well as details on how to configure the NicheStack TCP/IP 
 * networking stack and MicroC/OS-II Real-Time Operating System.  
 */

/*
 * Include files:
 * 
 * <stdlib.h>: Contains C "rand()" function.
 * <stdio.h>: Contains C "printf()" function.
 * includes.h: This is the default MicroC/OS-II include file.
 * simple_socket_servert.h: Our simple socket server app's declarations.
 * altera_avalon_pio_regs.h: Defines register-access macros for PIO peripheral.
 * alt_error_handler.h: Altera Error Handler suite of development error 
 * handling functions.
 */

/* <stdlib.h>: Contains C "rand()" function. */
#include <stdlib.h> 
#include <string.h>
/* <stdio.h>: Contains C "printf()" function. */
#include <stdio.h>  
#include <io.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>

/* MicroC/OS-II definitions */
#include "includes.h" 

/* common_defines */
#include "common_defines.h"

/* Device driver accessor macros for peripherial I/O component
 * (used for leds).) */
#include "altera_avalon_pio_regs.h"
#include "DE2_DRV11J_regs.h"
/* Simple Socket Server definitions */
#include "unimation_server.h"
#include "puma_states.h"
/* Altera Error Handler suite of development error 
 * handling functions. */                                                                   
#include "alt_error_handler.h"
#include "log_api.h"

#include "system.h"
#include "basic_io.h"
/* GPIO DRV11 driver */

//DEBUG TEST by disable real hardware IO
#define DEBUG_TEST	1
#define READ_ENCSET

#ifdef READ_ENCSET
	#define STARTUP_COUNT 30
	static unsigned char fake_startup_msg[STARTUP_COUNT] = {
	/* 0-1 :sync1 */	 0x55, 0x55,
	/* 2-3 :sync2 */  0xaa, 0xaa,
	/* 4-5: robot id */ 0x00, 0x00,
	/* 6-7: controller(sw id) 100 */ 0x64, 0x00,
	/* 8-9: options  : torque available 0x01, pots 0x02, pendant 0x04, foce 0x08, jstik_0x10, telop 0x20*/ 0x00, 0x00,
	/* 10-11: low : ADC, high DAC */     0x01, 0x01,
	/* 12-13: pio in, pio out */ 0x01, 0x01,
	/* 14-15: slu  ( low), integration */ 0x00, 0x00,
	/* 16-17: calibration mode: markFound */ 0x00, 0x00,

	/*18 - 19 encSet[0]*/ 0x01, 0x00,
	/*20 - 21 encSet[1]*/ 0x02, 0x00,
	/*22 - 23 encSet[2]*/ 0x03, 0x00,
	/*24 - 25 encSet[3]*/ 0x04, 0x00,
	/*26 - 27 encSet[4]*/ 0x05, 0x00,
	/*28 - 29 encSet[5]*/ 0x06, 0x00
 };
#else
	#define STARTUP_COUNT 18

	static unsigned char fake_startup_msg[STARTUP_COUNT] = {
	/* 0-1 :sync1 */	 0x55, 0x55,
	/* 2-3 :sync2 */  0xaa, 0xaa,
	/* 4-5: robot id */ 0x00, 0x00,
	/* 6-7: controller(sw id) 100 */ 0x64, 0x00,
	/* 8-9: options  : torque available 0x01, pots 0x02, pendant 0x04, foce 0x08, jstik_0x10, telop 0x20*/ 0x00, 0x00,
	/* 10-11: low : ADC, high DAC */     0x01, 0x01,
	/* 12-13: pio in, pio out */ 0x01, 0x01,
	/* 14-15: slu  ( low), integration */ 0x00, 0x00,
	/* 16-17: calibration mode: markFound */ 0x00, 0x00
 };
#endif


static INT32S EnqueueRecvMsg(INT8U cmd, INT8U status, INT8U *buffer, INT32S size);
static INT32S EnqueueRespMsg(INT8U cmd, INT8U status, INT8U *buffer, INT32S size);

static INT32S s_rev_q_id = 0;/* must be static (BSS) area*/
static INT32S s_rsp_q_id = 0;

#define MAX_ERROR_NUMBER		256
static char *error_string[MAX_ERROR_NUMBER]= {
         /* 0 */ "GPIO_ERROR_OK",
         /* 1 */ "GPIO_INVALID_HANDLE",
         /* 2 */ "GPIO_INVALID_VALUE",
         /* 3 */  "GPIO_SYNC_ERROR",
         /* 4 */ " GPIO_TIMOUT",
         /* 5 */ "GPIO_BAD_COMMAND",
         /* 6 */ "UNDEFINED",
         /* 7 */ "UNDEFINED",
         /* 8 */ "UNDEFINED",
         /* 9 */ "UNDEFINED",
         /* 10 */ "UNDEFINED",
         /* 11 */ "GPIO_ATTACH_ERROR",
         /* 12 */ "GPIO_DETACH_ERROR",
         /* 13 */ "GPIO_READ_FAILED",
         /* 14 */ "GPIO_WRITE_ERROR",
         /* 15 */ "N/A",
         /*16-31 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*32-47 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*49-63 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*64-79 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*80-95 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*96-111 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*112-127 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*128-143 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*144-159 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*160-175 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*176-191 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*192-207 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*208-223 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*224-239 */ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A",
         /*240-255*/ "N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A", "N/A","N/A", "N/A", "N/A",  "n/A"
};

/*
 * led_bit_toggle() sets or clears a bit in led_8_val, which corresponds
 * to 1 of 8 leds, and then writes led_8_val to a register on the Nios
 * Development Board which controls 8 LEDs, D0 - D7.
 * 
 */
 
void led_bit_toggle(OS_FLAGS bit)
{
  //  #ifdef LED_PIO_BASE
       IOWR_ALTERA_AVALON_PIO_DATA(LED_RED_BASE, bit);
       printf("Value for LED_PIO_BASE set to %d.\r\n", (INT8U)bit);
   // #endif
       IOWR_ALTERA_AVALON_PIO_DATA(SEG7_DISPLAY_BASE, (INT8U)bit);
   //        IOWR_ALTERA_AVALON_PIO_DATA(GPIO_SIGOUT_BASE, 0x1358);
    return;
}
 
/*
 *    UnimationSlaveTask receives messages from the Unimation Controller and 
 *    put in the recvQ and send to UnimationServerTask.
 * 
 */ 
 
int OpenUnimationController()
{
     int pio_fd =  open(GPIO_SIGIN_NAME, O_RDWR, 0);
     if ( pio_fd < 0)
     {
          fprintf(stderr, "Failed to open %s device,  UnimationSlaveTask  abort!!!\r\n", GPIO_SIGIN_NAME);
     }

     return pio_fd;
}
void CloseUnimationController( int fd)
{
    if ( fd > 0)
    {
        close (fd);
    }
}

void UnimationSlaveTask(void *arg)
{
   INT8U error_code;
   INT8U recv_buf[GPIO_DRV11J_MAX_BUF_SIZE];
   INT32S len;
   int pio_fd = *( int *) arg;


   pio_fd =  open(GPIO_SIGIN_NAME, O_RDWR, 0);

   fprintf(stdout,"UnimationSlaveTask: created!!\r\n");
   /* This is a task which does not terminate, so loop forever. */   
   while(1)
   {
       //seg7_show(SEG7_DISPLAY_BASE, i);

       /*Forward this command to Unimation controller */
      // len = read(pio_fd, recv_buf, GPIO_DRV11J_MAX_BUF_SIZE);
	   len = 0;
       if (len > 0 )
       {
            if (EnqueueRecvMsg(IND_RECEIVE_MSG, 0, recv_buf, len))
            {
                /* Send message index number */
                error_code = OSQPost(PumaStatesRecvQ, (void *)&stateRecvQ[s_rev_q_id]);

                seg7_show(SEG7_DISPLAY_BASE, len);

            }
         }
        else
        {
          //  i++;
        	/* Wait 50 milliseconds between pattern updates, to make the pattern slow
            * enough for the human eye, and more importantly, to give up control so
            * MicroC/OS-II can schedule other lower priority tasks. */
            OSTimeDlyHMSM(0,0,0,50);
        }
   }
}

/*
 * The UnimationClientTask()  receives and execute command from UnimationServerTask
 *  through UnimationCommandQ.
 * 
 * The task will read the UnimationCommandQ for an in-coming message command from the UnimationServerTask.
 */
void UnimationClientTask(void *arg)
{
    INT8U error_code;
    int   len;
    int   pio_fd = *( int *) arg;
    struct MESSAGE_Q* msg = NULL;
  
    fprintf(stdout,"[UnmationClientTask] created!!\r\n");
    if ( pio_fd < 0 )
    {
        fprintf(stderr,"[UnmationClientTask] failed to open %s exit!!\r\n", GPIO_SIGIN_NAME);
        return;
    }
  
    while(1)
    {
        /*check command from upper layer FIXME use Unblock mode */
    	msg = (struct MESSAGE_Q *)OSQPend(UnimationCommandQ, 100, &error_code);
    	//msg = (struct MESSAGE_Q *)OSQAccept(UnimationCommandQ, &error_code);
        if ( msg == NULL )
        {
    	   printf_error("Command message queue is empty");
    	   fprintf(stdout, "UnimationClientTask: message queue is empty");
           OSTimeDlyHMSM(0,0,0,50);
           continue;
        }
        //led_bit_toggle(0);
        switch (msg->cmd)
        {
            case CMD_ATTACH_REQ:
                fprintf(stdout,"[%s]: Execute attach command!\r\n",__FUNCTION__);
#ifndef DEBUG_TEST
                error_code = ioctl(pio_fd, IOC_DRV_ATTACH, 0 );
                led_bit_toggle(BIT_0); //FIXME later bug inside
                EnqueueRespMsg(CMD_ATTACH_RESP, error_code, NULL, 0);
#else
                fprintf(stdout,"[%s]Received CMD_DETATCH_REQ, Simulate and send back  CMD_ATTACH_RESP \r\n",__FUNCTION__);
                EnqueueRespMsg(CMD_ATTACH_RESP, 0, NULL, 0);   
#endif            
                fprintf(stdout,"[%s]: Send attach result %d[%s] to the server\r\n", __FUNCTION__,error_code, error_string[error_code]);

                break;
            case CMD_DETATCH_REQ:
             //   led_bit_toggle(BIT_1);
                fprintf(stdout,"[%s]: Execute detach command!\r\n",__FUNCTION__);
                error_code = ioctl(pio_fd, IOC_DRV_DETACH, 0 );

                EnqueueRespMsg(CMD_DETATCH_RESP, error_code, NULL, 0);
                fprintf(stdout,"[%s]: Send detach result %d[%s] to the server\r\n", __FUNCTION__,error_code, error_string[error_code]);

                break;

            case CMD_SIGNAL_REQ:
              //  led_bit_toggle(BIT_2);
                fprintf(stdout,"[%s]: Execute signal command!\r\n",__FUNCTION__);
                error_code = ioctl(pio_fd, IOC_SEND_SIGNAL, &msg->data[0]);

                EnqueueRespMsg(CMD_SIGNAL_RESP, error_code, NULL, 0);
                fprintf(stdout,"[%s]: Send signal result %d[%s] to the server\r\n", __FUNCTION__,error_code, error_string[error_code]);

                break;

            case CMD_READ_REQ: /* TBD : Block read directly*/
                fprintf(stdout,"[%s]: Execute read command!\r\n",__FUNCTION__);
#ifndef DEBUG_TEST
                len = read(pio_fd, msg->data, GPIO_DRV11J_MAX_BUF_SIZE);
                error_code = ( len > 0) ? GPIO_ERROR_OK : GPIO_READ_FAILED;

                EnqueueRespMsg(CMD_READ_RESP, error_code, msg->data, len);
                fprintf(stdout,"[%s]: Send read result %d[%s] to the server\r\n", __FUNCTION__,error_code, error_string[error_code]);
#else
                len = STARTUP_COUNT;
                EnqueueRespMsg(CMD_READ_RESP, 0, fake_startup_msg, len);
#endif

                // led_bit_toggle(BIT_3);
                break;

            case CMD_WRITE_REQ: /* CMD_LEDS_BIT_4_TOGGLE*/
                fprintf(stdout,"[%s]: Execute write command!\r\n",__FUNCTION__);
#ifndef DEBUG_TEST
                len = write(pio_fd, msg->data, msg->u16Length );
                error_code = 0;
                if (len != msg->u16Length)
                {   error_code = GPIO_WRITE_ERROR;
                    printf_error("[%s]: Write data error!", __FUNCTION__);
                }

                EnqueueRespMsg(CMD_WRITE_RESP, error_code, NULL, 0);
                fprintf(stdout,"[%s]: Send write result %d[%s] to the server\r\n", __FUNCTION__,error_code, error_string[error_code]);
#else

                EnqueueRespMsg(CMD_WRITE_RESP, 0, NULL, 0);
#endif

               // led_bit_toggle(BIT_4);
                break;

            case CMD_IOCTL_REQ:
#ifndef DEBUG_TEST
                len = (int)( msg->data[0] + (msg->data[1] << 8));
                fprintf(stdout,"[%s]: Execute IO_CTRL command! 0x%04x\r\n",__FUNCTION__, *(unsigned short*)&msg->data[2]);
                error_code =  ioctl(pio_fd, msg->data[2], &(msg->data[0]));
                fprintf(stdout,"[%s]: Send IO_CTRL result %d[%s] to server\r\n", __FUNCTION__,error_code, error_string[error_code]);
                if (error_code != GPIO_ERROR_OK)
                {
                	printf_error("IOCTL error: %d", error_code);
                 //   alt_UnimationErrorHandler(error_code, "IOCTL error");
                }
                EnqueueRespMsg(CMD_IOCTL_RESP, error_code, NULL, 0);
#else
                EnqueueRespMsg(CMD_IOCTL_RESP, 0, NULL, 0);
#endif
                break;

            default:
            	/* Discard unknown LED commands. */
                printf_error("[%s]: Unknown command 0x%02x!",__FUNCTION__, msg->cmd);
                error_code = GPIO_BAD_COMMAND;
                EnqueueRespMsg(CMD_UNKNOWN_RESP, error_code, NULL, 0);
                fprintf(stdout,"[%s]: Send result %d[%s] to server\r\n", __FUNCTION__,error_code, error_string[error_code]);
                break;

             /*
             case CMD_BUFFER_READ_REQ;
             case CMD_BUFFER_WRITE_REQ;
             */
        } /* switch led_command */

          error_code = OSQPost(UnimationResponseQ, (void *) &respQ[s_rsp_q_id]);
          if (error_code == OS_Q_FULL)
          {
            printf_error("Response message queue is FULL! drop the message!");
          }

          OSTimeDlyHMSM(0,0,0,50);

    /* For here, check there is any indication command from lower layer*/
       } /* while(1) */
} 

/*            EnqueueRecvMsg
 * return 0: empty; 1: Dequeued message length
 */
static INT32S EnqueueRecvMsg(INT8U cmd, INT8U status,
        INT8U *buffer, INT32S buf_size)
{
    INT32S len =0;

    s_rev_q_id = (s_rev_q_id +1 )% UNIMATION_MSG_Q_SIZE;
    /* need considering status byte*/
    len = (buf_size <= UNIMATION_MSG_BUF_SIZE-2) ?  buf_size : (UNIMATION_MSG_BUF_SIZE - 2);
    stateRecvQ[s_rev_q_id].u16Length = len+2; // size +2 data length in the frame header
    stateRecvQ[s_rev_q_id].cmd  = cmd;
    stateRecvQ[s_rev_q_id].cmdExt = 0x00;
    stateRecvQ[s_rev_q_id].data[0] = status;
    stateRecvQ[s_rev_q_id].data[1] = 0x00; // reserved.

    if ( buffer && (len > 0))
    {
      memcpy(&(stateRecvQ[s_rev_q_id].data[2]), buffer,len );
    }

     return len;
}

/*            EnqueueRespMsg
 * return 0: empty; 1: Enqueue response message from the client to the server.
 */
static INT32S EnqueueRespMsg(INT8U cmd, INT8U status,
        INT8U *buffer, INT32S buf_size)
{
    INT32S len =0;

    s_rsp_q_id = (s_rsp_q_id +1 )% UNIMATION_MSG_Q_SIZE;
    /* need considering status byte*/
    len = (buf_size <= UNIMATION_MSG_BUF_SIZE-2) ? buf_size : (UNIMATION_MSG_BUF_SIZE-2);
    respQ[s_rsp_q_id].u16Length = len + 2; // size +2 data length in the frame header
    respQ[s_rsp_q_id].cmd  = cmd;
    respQ[s_rsp_q_id].cmdExt = 0x00;
    respQ[s_rsp_q_id].data[0] = status;
    respQ[s_rsp_q_id].data[1] = 0x00;

    if ( buffer && (len > 0))
    {
        memcpy(&(respQ[s_rsp_q_id].data[2]), buffer,len );
    }

     return len;
}
/******************************************************************************
*                                                                             *
* License Agreement                                                           *
*                                                                             *
* Copyright (c) 2006 Altera Corporation, San Jose, California, USA.           *
* All rights reserved.                                                        *
*                                                                             *
* Permission is hereby granted, free of charge, to any person obtaining a     *
* copy of this software and associated documentation files (the "Software"),  *
* to deal in the Software without restriction, including without limitation   *
* the rights to use, copy, modify, merge, publish, distribute, sublicense,    *
* and/or sell copies of the Software, and to permit persons to whom the       *
* Software is furnished to do so, subject to the following conditions:        *
*                                                                             *
* The above copyright notice and this permission notice shall be included in  *
* all copies or substantial portions of the Software.                         *
*                                                                             *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR  *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,    *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER      *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING     *
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER         *
* DEALINGS IN THE SOFTWARE.                                                   *
*                                                                             *
* This agreement shall be governed in all respects by the laws of the State   *
* of California and by the laws of the United States of America.              *
* Altera does not recommend, suggest or require that this reference design    *
* file be used in conjunction or combination with any other product.          *
******************************************************************************/
