/****************************************************************************/
/* 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 "p24Fxxxx.h"
#include "GenericTypeDefs.h"

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


static USHORT	gfNumSamps=10, gfSampPeriod=100, gfIgnoreSamps=10, gfCtrl=1;
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};

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

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

void gfInit(void)
{
    setGfChan(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:
	  if (((val&3) == 2) && (gfCtrl & 1))
	      setGfChan(GF_TEST_CHAN);

	  if (((val&3) == 1) && (gfCtrl & 2))
	      setGfChan(0);

	  gfCtrl = (val & 0x3);
	  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);

	gfChan++;				/* Increment chan number*/

	/* Check for end of channels (test chans or normal chans) */
	if (gfChan > ((gfCtrl & 2) ? GF_TEST_END : GF_NORM_END))
	{					/* If end, roll over to start*/
	    gfChan = (gfCtrl & 1) ? 0 : GF_TEST_CHAN;
	    gfSeq++;				/* and increment the seq num */
	}

	setGfChan(gfChan);
    }
}