/****************************************************************************/
/* Copyright 2014 MBARI                                                     */
/* MBARI Proprietary Information. All rights reserved.                      */
/* Written 30jan2014, Robert Herlien (rah)				    */
/****************************************************************************/

#include <stdio.h>
#include "misc.h"
#include "gf.h"
#include "adc.h"
#include "sys_timer.h"
#include "serial.h"
#include "p24Fxxxx.h"
#include "GenericTypeDefs.h"

#define MIN_SAMP_PERIOD	  10
#define MAX_SAMP_PERIOD	65535
#define MAX_IGNORE	65535
#define GF_MAX_SAMPLES	65535

//#define DEBUG_GF	1

static USHORT	gfNumSamps=10, gfSampPeriod=100, gfIgnoreSamps=10, gfCtrl=0;
static USHORT	gfSeq=0, gfChan=0, gfSampTicks=10;

static USHORT	gfSamps=0, gfIgnored=0;
static ULONG	gfAccum=0L, gfNextTick=0L;

static USHORT	gfResult[NUM_GF_CHANS] = {0, 0, 0, 0, 0, 0,};
static USHORT	gfSel[NUM_GF_CHANS] = {0x4, 0x8, 0x10, 0x20, 0x40, 0x80};
static USHORT	gfCtrlBit[NUM_GF_CHANS] = {0x1, 0x2, 0x4, 0x8, 0x10, 0x20};

#ifdef DEBUG_GF
static char	gfDbgBuf[256];
#endif

void setGfChan(UINT chan)
{
    if (chan < NUM_GF_CHANS)
    {
	LATB = 0;
	tmrDelayMs(10);
	LATB = gfSel[chan];
	gfChan = chan;
    }

    gfIgnored = 0;
    gfSamps = 0;
    gfAccum = 0L;
}

void nextGfChan(void)
{
    UINT	i;

    if (gfCtrl == 0)
    {
	LATB = 0;
	return;
    }

    for (i = gfChan+1; i < NUM_GF_CHANS; i++)
	if (gfCtrl & gfCtrlBit[i])
	{
	    setGfChan(i);
	    return;
	}

    gfSeq++;

    for (i = 0; i <= gfChan; i++)
	if (gfCtrl & gfCtrlBit[i])
	{
	    setGfChan(i);
	    return;
	}

    LATB = 0;
}

void setGfCtrl(UINT ctrl)
{
    gfCtrl = ctrl & 0x3f;

    if ((gfCtrl & gfCtrlBit[gfChan]) == 0)
	nextGfChan();
}

void gfInit(void)
{
    LATB = 0;
    gfCtrl = 0;
    gfIgnored = 0;
    gfSamps = 0;
    gfAccum = 0L;
    gfSeq = 0;
    gfNextTick = tmrGetTicks();
}

UINT gfRegRead(UINT address)
{	
    if ((address >= GF_RESULT_START) && (address < (GF_RESULT_START+NUM_GF_CHANS)))
	return(gfResult[address - GF_RESULT_START]);

    switch(address)
    {
      case GF_NUM_SAMPS:
	return(gfNumSamps);

      case GF_SAMP_PERIOD:
	  return(gfSampPeriod);

      case GF_IGNORE_SAMPS:
	  return(gfIgnoreSamps);

      case GF_CTRL:
	  return(gfCtrl);

      case GF_SEQ:
	  return(gfSeq);

      case GF_CHAN:
	  return(gfChan);
    }
    return(0);
}

eMBErrorCode gfRegWrite(UINT address, UINT val)
{
    switch(address)
    {
      case GF_NUM_SAMPS:
	  if (val && (val <= GF_MAX_SAMPLES))
	      gfNumSamps = val;
	  else
	      return(MB_EINVAL);
	  break;
	  
      case GF_SAMP_PERIOD:
	  if ((val >= MIN_SAMP_PERIOD) && (val <= MAX_SAMP_PERIOD))
	  {
	      gfSampTicks = (val + MS_PER_TICK/2)/MS_PER_TICK;
	      gfSampPeriod = gfSampTicks * MS_PER_TICK;
	  }
	  else
	      return(MB_EINVAL);
	  break;

      case GF_IGNORE_SAMPS:
	  if (val <= MAX_IGNORE)
	      gfIgnoreSamps = val;
	  else
	      return(MB_EINVAL);
	  break;

      case GF_CTRL:
	  setGfCtrl(val & 0x3f);
	  break;

      case GF_SEQ:
      case GF_CHAN:
	  return(MB_EINVAL);

      default:
	  if (address >= GF_RESULT_START)
	      return(MB_EINVAL);
	  else
	      return(MB_ENOREG);
    }

    return(MB_ENOERR);
}

void gfTask(void)
{
    ULONG curTick = tmrGetTicks();

    /* See if time to update GF Sample	*/
    if ((gfCtrl == 0) || ((long)(gfNextTick - curTick) > 0L))
	return;

    /* Time to update GF sample		*/
    gfNextTick = curTick + gfSampTicks;

    if (gfIgnored < gfIgnoreSamps)		/* Still ignoring?	*/
	if (++gfIgnored < gfIgnoreSamps)	/* If so, done ignoring?*/
	    return;				/* If not done, return	*/

    /* We're sampling GF channel gfhan	*/
    gfAccum += adsConvertGFchan();
    gfSamps++;

    if (gfSamps >= gfNumSamps)			/* Done sampling?	*/
    {
	if (gfSamps > 0)			/* Yes, store result	*/
	    gfResult[gfChan] = (USHORT)(gfAccum / gfSamps);

#ifdef DEBUG_GF
	sprintf(gfDbgBuf, "GF chan %u = %u\r\n", gfChan, gfResult[gfChan]);
	serPutString(CFG_SERPORT, gfDbgBuf);
#endif

	nextGfChan();
    }
}