///
/// @file test-snloop.c
/// @authors headley (klh)
/// @date 07 dec 2012
/// 
/// xFOCE CLI sensor node control utility

/// utility exposing basic sensor node ops
/// Use when gateway is not running;
/// Use gq client when gateway is running


/////////////////////////
// Terms of use 
/////////////////////////
/*
 Copyright Information
 
 xFOCE - software for ocean acidification experiments
 Copyright 2002-2013 MBARI
 Monterey Bay Aquarium Research Institute, all rights reserved.
 
 Terms of Use
 
 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 3 of the License, or
 (at your option) any later version. You can access the GPLv3 license at
 http://www.gnu.org/licenses/gpl-3.0.html
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details 
 (http://www.gnu.org/licenses/gpl-3.0.html)
 
 MBARI provides the documentation and software code "as is", with no warranty,
 express or implied, as to the software, title, non-infringement of third party 
 rights, merchantability, or fitness for any particular purpose, the accuracy of
 the code, or the performance or results which you may obtain from its use. You 
 assume the entire risk associated with use of the code, and you agree to be 
 responsible for the entire cost of repair or servicing of the program with 
 which you are using the code.
 
 In no event shall MBARI be liable for any damages, whether general, special,
 incidental or consequential damages, arising out of your use of the software, 
 including, but not limited to, the loss or corruption of your data or damages 
 of any kind resulting from use of the software, any prohibited use, or your 
 inability to use the software. You agree to defend, indemnify and hold harmless
 MBARI and its officers, directors, and employees against any claim, loss, 
 liability or expense, including attorneys' fees, resulting from loss of or 
 damage to property or the injury to or death of any person arising out of the 
 use of the software.
 
 The MBARI software is provided without obligation on the part of the 
 Monterey Bay Aquarium Research Institute to assist in its use, correction, 
 modification, or enhancement.
 
 MBARI assumes no responsibility or liability for any third party and/or 
 commercial software required for the database or applications. Licensee agrees 
 to obtain and maintain valid licenses for any additional third party software 
 required.
 */

/////////////////////////
// Headers 
/////////////////////////
#include <stdio.h>
#include <string.h>
#include <unistd.h>
// for basename
#include <libgen.h>
// 

#include "sensorNode.h"

/////////////////////////
// Macros
/////////////////////////
#define PON    "pon"
#define POFF   "poff"
#define STATUS "status"
#define INFO   "info"
#define BLINKY "blinky"
#define LOOP   "loop"

#define GW_NULL        NULL
#define DFL_PLATFORM   0
#define DFL_INSTANCE   0
#define DFL_SID        1000
#define DFL_RETRIES    3
#define DFL_POW_POLICY 0
#define DFL_PON_DELAY  0
#define DFL_TIMEOUT    3000
#define DFL_SEG_SZ     10000
#define DFL_SEG_COUNT  10
#define DFL_LOOPBACK   false
#define SSN_ADC_CHANNELS 7

/////////////////////////
// Imports
/////////////////////////
extern struct map_entry *test_errmaps[];

/////////////////////////
// Exports
/////////////////////////

/////////////////////////
// Type Definitions
/////////////////////////
typedef struct snloop_cfg_s{
    SensorNodeHandle snh;
    RemSerHandle rsh;
    io_port_cfg sn_io;
    io_port_cfg snp_io;
    int debug_level;
	bool snlib_debug_en;
	int op_count;
	char *op[10];
	char *op_args[10];
}snloop_cfg;

/////////////////////////
// Module Global Variables
/////////////////////////
static int verbose=0;
#define adVolts (3.0/65535)

static adc_config_t s_adc_cfg[SSN_ADC_CHANNELS]={
    {{2.0*adVolts,0.0,0.0},  ADC_CAL_LIN,"chan 0", "device current (A)", adc_scale},
    {{16.0*adVolts,0.0,0.0}, ADC_CAL_LIN,"chan 1", "device voltage (V)", adc_scale},
    {{0.667*adVolts,0.0,0.0},ADC_CAL_LIN,"chan 2", "port[0] current [A]",adc_scale},
    {{0.667*adVolts,0.0,0.0},ADC_CAL_LIN,"chan 3", "port[1] current [A]",adc_scale},
    {{0.667*adVolts,0.0,0.0},ADC_CAL_LIN,"chan 4", "port[2] current [A]",adc_scale},
    {{2.0*adVolts,0.0,0.0},  ADC_CAL_LIN,"chan 5", "port[3] current [A]",adc_scale},
    {{6.1*adVolts,4.0,0.0},  ADC_CAL_LIN,"chan 6", "4-20 mA current [A]",adc_scale}//,
    //{{1.0,0.0,0.0},          ADC_CAL_RAW,"chan 7", "unused [adc counts]",adc_scale}
};

static adc_config_t *adc_cfg[SSN_ADC_CHANNELS]={
    &s_adc_cfg[0],
    &s_adc_cfg[1],
    &s_adc_cfg[2],
    &s_adc_cfg[3],
    &s_adc_cfg[4],
    &s_adc_cfg[5],
    &s_adc_cfg[6]
};

/////////////////////////
// Function Definitions
/////////////////////////

/// @fn void printUsage(char *program)
/// @brief print use message.
/// Print use message, prepended with message.
/// @param[in] program program name
/// @return none
void 
print_usage(char *program)
{
	fprintf(stderr,"\nUsage: %s [options]\n\n",program);	
	fprintf(stderr,"Options:\n");	
    fprintf(stderr,"  -n <ports>  : turn port[s] ON\n");
    fprintf(stderr,"  -f <ports>  : turn port[s] OFF\n");
    fprintf(stderr,"  -b <s>      : set blinky state [0:off 1:on 2:blink]\n");
    fprintf(stderr,"  -i          : get sensor node info\n");
	fprintf(stderr,"  -q          : query status\n");
    fprintf(stderr,"  -l <loops,bytes,delay>  : loopback test\n");
    fprintf(stderr,"\n");
    fprintf(stderr,"  -s [<dev>,<term>] : SensorNode terminal info\n");
    fprintf(stderr,"  where\n");
    fprintf(stderr,"    <dev >      : <device_path>\n");
    fprintf(stderr,"    <term>      : <speed>,<par>,<csize>,<stopbits>\n");
    fprintf(stderr,"\n");
    fprintf(stderr,"  -d <level>  : debug level (d,i,w,e,f,[s])\n");
    fprintf(stderr,"  -h          : print help message\n");
    fprintf(stderr,"  -v          : verbose output\n");
    fprintf(stderr,"\n");
	fprintf(stderr,"Example(s):\n");
	fprintf(stderr,"  %s -s /dev/ttyO2 -p 1,38400 -l 10,64,100\n",program);
	fprintf(stderr,"\n\n");	
}

/// @fn void process_args(int argc, char **argv)
/// @brief process command line args.
/// Get command line options for this module.
/// @param[in] argc number of arguments
/// @param[in] argv array of command line arguments
/// @return returns 0 on success, -1 otherwise
static int 
process_args(int argc, char **argv, snloop_cfg *cfg)
{
	int opt;
    char tty_ssrid[BUFFER_32];
    char sn_ssrid[BUFFER_32];
    char snp_ssrid[BUFFER_32];
	sys_resource_id tty_srid=0;
	sys_resource_id sn_srid=0;
	sys_resource_id snp_srid=0;
	char debug_c='i';

    char **sn_parms = NULL;
    char **snp_parms = NULL;
    char **tty_parms = NULL;
    int sn_toks = 0;
    int snp_toks = 0;
    int tty_toks = 0;
    char *scpy=NULL;
    char *tp=NULL;

	// initialize globals here...
	while ( (opt = getopt(argc,argv,"l:n:f:b:iqd:s:p:t:vh")) != -1) {
		switch (opt) {

            case 'd':
                debug_c=optarg[0];
                switch (debug_c) {
                    case 'd':
                        cfg->debug_level=MDL_DEBUG;
                        break;
                    case 'i':
                        cfg->debug_level=MDL_INFO;
                        break;
                    case 'w':
                        cfg->debug_level=MDL_WARN;
                        break;
                    case 'e':
                        cfg->debug_level=MDL_ERROR;
                        break;
                    case 'f':
                        cfg->debug_level=MDL_FATAL;
                        break;
                    case 's':
                        cfg->snlib_debug_en=TRUE;
                        break;
                    default:
                        break;
                }
                if (strstr(optarg,"s")) {
                    cfg->snlib_debug_en=TRUE;
                }
                break;
            case 'n':
                cfg->op[cfg->op_count]=PON;
                cfg->op_args[cfg->op_count]=optarg;
                cfg->op_count++;
                break;
            case 'f':
                cfg->op[cfg->op_count]=POFF;
                cfg->op_args[cfg->op_count]=optarg;
                cfg->op_count++;
                break;
            case 'b':
                cfg->op[cfg->op_count]=BLINKY;
                cfg->op_args[cfg->op_count]=optarg;
                cfg->op_count++;
                break;
            case 'i':
                cfg->op[cfg->op_count]=INFO;
                cfg->op_args[cfg->op_count]=NULL;
                cfg->op_count++;
                break;
            case 'q':
                cfg->op[cfg->op_count]=STATUS;
                cfg->op_args[cfg->op_count]=NULL;
                cfg->op_count++;
                break;
            case 'p':
                snp_toks=io_parse_snp_cfg(optarg, NULL, &cfg->snp_io);
                break;
            case 's':
                sn_toks=io_parse_term(optarg, NULL, &cfg->sn_io);
                break;
            case 'l':
                cfg->op[cfg->op_count]=LOOP;
                cfg->op_args[cfg->op_count]=optarg;
                cfg->op_count++;
                break;
			case 'v':
				verbose=1;
				break;
            case 'h':
                print_usage(basename(argv[0]));
                exit(-1);
			default:
				fprintf(stderr,"\ninvalid argument [%c]\n",opt);
				print_usage(basename(argv[0]));
				exit(-1);
		}
	}
}

static int
s_get_status(snloop_cfg *cfg, snode_stat_t *dest)
{
	
    int retval=-1;
    
    if (cfg && cfg->snh && &cfg->rsh) {
 
        
            // read power vector into status struct
            if(snGetPowerVector(cfg->snh, &dest->port_state)==OK){
            }// else could not read power vector
            
            // read ADC channels into status struct
            if(snAnalogChans(cfg->snh, 0, SSN_ADC_CHANNELS, dest->adc_raw)==OK){
 
                // convert all ADC channel raw values (adc_raw), store in status struct (adc_scaled)
                adc_convert(adc_cfg, dest->adc_scaled, dest->adc_raw, 0, SSN_ADC_CHANNELS);

            }// else could not read ADC channels
     }// else invalid arg
    else{
        MDEBUG(MD_TEST_TRACE,"invalid arg config[%p]\n", cfg);
    }
    
	return retval;
}

static void
s_show_status(int indent, snode_stat_t *stat, adc_config_t **ch_cfg)
{
    if (stat && ch_cfg) {
        printf("%*s[power vector: %1X]\n",indent,(indent?" ":""), stat->port_state);
        int i=0;
        printf("%*s[adc channels]\n",indent,(indent?" ":""));
        for (i=0; i<SSN_ADC_CHANNELS; i++) {
            printf("%*s[adc[%d]: name[%s] raw[%5u] scaled[ %.4E (%s)] ]\n",indent,(indent?" ":""),
                   i,
                   ch_cfg[i]->name,
                   stat->adc_raw[i],
                   stat->adc_scaled[i],
                   ch_cfg[i]->units );
        }
        printf("\n");
        //        adc_config_idump(ch_cfg[i], indent);
    }
}

static int
s_get_info(snloop_cfg *cfg, snode_info_t *dest)
{
    
    int retval=0;
    if (cfg && cfg->snh && cfg->rsh) {
        
            // read power vector into info struct
        
            if(snGetSerialNum(cfg->snh, &dest->serial_number)==OK){
            }// else error
            else{
                retval|=0x1;
            }
            
            // read SW revision into info struct
            if(snGetSoftwareRev(cfg->snh, &dest->sw_rev)==OK){
            }// else error
            else{
                retval|=0x2;
            }
            
            // read board config into info struct
            if(snGetBoardConfig(cfg->snh, &dest->board_cfg)==OK){
            }// else error
            else{
                retval|=0x4;
            }

            // read HW config into info struct
            if(snGetHwConfig(cfg->snh, &dest->hw_cfg)==OK){
            }// else error
            else{
                retval|=0x8;
            }
    }// else invalid arg
    else{
        MDEBUG(MD_TEST_TRACE,"invalid arg\n");
    }

    return retval;
}

static void
s_show_info(int indent, snode_info_t *info)
{
    if (info) {
        printf("%*s[serial number  : %hu]\n",indent,(indent?" ":""), info->serial_number);
        printf("%*s[software rev   : %hu]\n",indent,(indent?" ":""), info->sw_rev);
        printf("%*s[board config   : %04X]\n",indent,(indent?" ":""), info->board_cfg);
        printf("%*s[hardware config: %04X]\n",indent,(indent?" ":""), info->hw_cfg);
    }
}

static int
s_set_power(snloop_cfg *cfg,  const char *ports, snlib_power_t state)
{
    
    int retval=0;
    if (cfg && cfg->rsh && ports) {
        
            if (strstr(ports,"all" )>0) {
                fprintf(stderr,"%s - switching all [%s]\n",__FUNCTION__,ports);
                snPower(cfg->snh, 0, state);
                snPower(cfg->snh, 1, state);
                snPower(cfg->snh, 2, state);
                snPower(cfg->snh, 3, state);
            }else{
                int i=0;
                uint16_t port=0x00;
                
                for(i=0;i<strlen(ports);i++){
                    switch (ports[i]) {
                        case '0':
                        case '1':
                        case '2':
                        case '3':
                            port = ports[i]-'0';
                            fprintf(stderr,"%s - switching ports[%d][%c] port[%hu]\n",__FUNCTION__,i,ports[i],port);

                            snPower(cfg->snh, port, state);
                            break;
                        default:
                            // else ignore
                            break;
                    }
                }
            }
    }else{
        // else invalid arg
        MDEBUG(MD_TEST_TRACE,"invalid arg ports[%p]\n", ports);
    }
   return retval;
}

static int
s_set_blinky(snloop_cfg *cfg, const char *sstate)
{
    
    int retval=0;
    if (sstate && cfg && cfg->snh) {
        uint16_t bmode=0xFF;
        if (sscanf(sstate,"%hu",&bmode)==1) {
            snBlinky(cfg->snh, (BlinkyState)bmode);
        }// else invalid format
        
    } else{
        MDEBUG(MD_TEST_TRACE,"invalid arg\n");
        retval=-1;
    }
    
    return retval;
}

static int
s_loop_nwr(snloop_cfg *cfg, byte *wbuf, byte *rbuf, uint32_t len, uint64_t delay)
{
    int retval=0;
    
    //md_set(MDI_XF_IO_TERM, MDL_DEBUG);
    char *pname=cfg->sn_io.dev;
    int wretries=10;
    int rretries=10;
    int err=0;
    if (cfg && cfg->snh && cfg->rsh && wbuf && rbuf && len>0) {
        
        // dump anything currently in the input and output
        snSerIFlush(cfg->rsh);
        snSerOFlush(cfg->rsh);
        
        MDEBUG(MD_LOOP,"%s: writing [%d] w/r[%p/%p]\n",pname,len,wbuf,rbuf);
        size_t wcount = (size_t)snSerWrite(cfg->rsh, wbuf, len);
        err=errno;
        if (wcount>0 && wcount<=len) {
            
            MDEBUG(MD_LOOP,"%s: delaying [%llu]\n",pname,delay);
            if (delay>0L) {
                delay_msec(delay);
            }
            
            MDEBUG(MD_LOOP,"%s: reading wc[%zu]\n",pname,wcount);
            size_t rcount = 0;
            while(rretries-- > 0){
                rcount=(size_t)snSerRead(cfg->rsh, rbuf,wcount);
                err=errno;
                if ( rcount>0) {
                    break;
                }else{
                    if (rcount<0) {
                        MDEBUG(MD_LOOP,"%s: fread err ret[%zu] err[%d/%s] (retrying)\n",pname,rcount,err,strerror(err));
                    }
                    delay_msec(75);
                    rcount=0;
                }
            }
            
            MDEBUG(MD_LOOP,"%s: read done: rc[%zd] ret[%d] err[%d/%s]\n",pname,rcount,rretries,err,strerror(err));
            int j=0;
            if (rcount>0 && rcount<=len) {
                MDEBUG(MD_LOOP,"%s: comparing rc[%zd]\n",pname,rcount);
                int i=0;
                for (i=0; i<rcount; i++) {
                    if (rbuf[i] == wbuf[i]) {
                        retval++;
                    }
                    //                    else{
                    //                        if (j==0) {
                    //                            fprintf(stderr,"\n%s ",io_port->properties->name);
                    //                        }
                    //                        fprintf(stderr,"[%02X/%02X] ",wbuf[i],rbuf[i]);
                    //                        if (j!=0 && (j+1)%8==0) {
                    //                            fprintf(stderr,"\n%s ",io_port->properties->name);
                    //                        }
                    //                        j++;
                    //                    }
                }
                //                fprintf(stderr,"\n");
                MDEBUG(MD_LOOP,"%s: matched [%d/%u]\n",pname,retval,len);
            }else{
                // else invalid read
                MDEBUG(MD_LOOP,"%s: invalid snSerRead returned[%zu]\n",pname,rcount);
                retval=-3;
            }
            fprintf(stderr,"%s: len[%u] wc[%zd] rc[%zd] rval[%d]\n",pname,len, wcount,rcount,retval);
            
        }else{
            // else invalid write
            MDEBUG(MD_LOOP,"%s: invalid snSerWrite returned[%zu] er[%d/%s]\n",pname,wcount,err,strerror(err));
            retval=-2;
        }
    }else{
        fprintf(stderr,"%s:%d - invalid arg iop[%s] wb[%p] rb[%p] len[%u]\n",__FUNCTION__,__LINE__,pname,wbuf,rbuf,len);
        retval=-1;
    }
    //md_set(MDI_XF_IO_TERM, MDL_INFO);
    
    return retval;
}

static int
s_loop(snloop_cfg *cfg, const char *args)
{

    srand(time(NULL));
    
    uint32_t bsize=64;
    uint32_t loops=1;
    uint64_t delay_msec=500;
    int test=0;
    if (args) {
        test=sscanf(args,"%u,%u,%llu",&loops,&bsize,&delay_msec);
    }
    fprintf(stderr,"scan[%d] n[%u] sz[%u] d[%llu]\n",test,loops,bsize,delay_msec);
    
    
    byte *wbuf=(byte *)malloc(bsize);
    byte *rbuf=(byte *)malloc(bsize);
    
    while(loops-- >0){
        memset(wbuf,0,bsize);
        memset(rbuf,0,bsize);
        loop_rand_fill(wbuf,bsize);
        s_loop_nwr(cfg, wbuf, rbuf, bsize, delay_msec);
    }
    free(wbuf);
    free(rbuf);
}

static int
s_init(snloop_cfg *cfg)
{
    int retval=-1;
    
    initSensorNodeLib();
    snSetDebug((MBool)cfg->snlib_debug_en);
    
    cfg->snh = openSensorNode(cfg->sn_io.dev, io_speed2i(cfg->sn_io.term.cfg_speed), 1);
    cfg->rsh =	snOpenSerial( cfg->snh, cfg->snp_io.index);
    if (cfg->snh && cfg->rsh) {
        retval=0;
    }else{
        fprintf(stderr,"err: snh[%p] rsh[%p]\n",cfg->snh,cfg->rsh);
    }
    
    return retval;
}
/// @brief main function entry point
/// @param[in] argc number of arguments
/// @param[in] argv array of command line arguments
/// @return returns 0 if successful, -1 otherwise
int 
main(int argc, char **argv)
{
	int test=0;
    int retval=0;
	snloop_cfg config={
        NULL,          // SensorNodeHandle
        NULL,          // RemSerHandle
        {0,0,{0},{{0},{0},B115200,CS8,STOPB_1,PAR_N}},// snode IO
        {0,0,{0},{{0},{0},B9600,CS8,STOPB_1,PAR_N}}, // snport IO
        MDL_INFO,      // debug level
		FALSE,         // snlib debug enable
		0,             // op_count
		{0},           // op names
		{0}            // op args (strings)
	};
	
	process_args(argc,argv, &config);
	
    md_set(MDI_TEST_TRACE, config.debug_level);
    md_set(MDI_LOOP, config.debug_level);
    //md_set(MDI_XF_IO_SNLIB, config.debug_level);
    //md_set(MDI_SNLIB_IO, config.debug_level);
    //md_set(MDI_DEVICECRAFT, config.debug_level);

    s_init(&config);
    
	if (config.rsh) {
		int i=0;
  
		while (i < config.op_count) {
			//printf("op[%d][%s] args[%s]\n",i,config.op[i],config.op_args[i]);
            if (strcasecmp(config.op[i],STATUS)==0) {
                snode_stat_t sn_stat={0};
                s_get_status(&config, &sn_stat);
                s_show_status(5,&sn_stat, adc_cfg);
            }
            if (strcasecmp(config.op[i],INFO)==0) {
                snode_info_t sn_info={0};
                s_get_info(&config, &sn_info);
                s_show_info(5,&sn_info);
            }
            if (strcasecmp(config.op[i],PON)==0) {
                s_set_power(&config,config.op_args[i], SNLIB_PON);
            }
            if (strcasecmp(config.op[i],POFF)==0) {
                s_set_power(&config,config.op_args[i], SNLIB_POFF);
            }
            if (strcasecmp(config.op[i],BLINKY)==0) {
                s_set_blinky(&config,config.op_args[i]);
            }
            if (strcasecmp(config.op[i],LOOP)==0) {
                s_loop(&config,config.op_args[i]);
            }

			i++;
		}
        snCloseSerial(config.rsh);
        closeSensorNode(config.snh);
	}else {
		fprintf(stderr,"\n  IO configuration failed\n\n");
        retval=-1;
	}

	//printf(" xmsg test: [%s - %hu:%s]\n",RETURNSTR(sw_test),xferrno,lmapstr(xferrno,test_errmaps));

	return 0;
}