#ifndef __BORLANDC__
#include <msp430x14x.h>
#endif
#include <stdio.h>

#include "basic.h"
#include "config.h"
#include "util.h"
#include "cmdparse.h"
#include "outfmt.h"
#include "eventq.h"
#include "timer.h"
#include "sensor.h"
#include "gpio.h"
#include "adc12.h"
#include "errnum.h"

#ifdef CONFIG_INCLUDE_GFI

/*
 * Ground Fault Current module documentation
 *
 * [1] Official GFI algorithm from Ed Mellinger:
 *
 * 1) assert CHECK_HIGH_SIDE
 * 2) wait for a settling time (settable between 1 and 60 seconds 
 *    in 1 sec increments)
 * 3) read A/D channels for HIGH_SIDE_FAULT and LOW_SIDE_FAULT
 *
 * 4) de-assert CHECK_HIGH_SIDE
 * 5) wait for settling time (same value as in Step 2)
 * 6) read A/D channels for HIGH_SIDE_FAULT and LOW_SIDE_FAULT
 * 7) subtract Low_Side value in Step 3, from Low_Side value in Step 6.  
 *    Subtract High_Side value in Step 6, from High_Side value in Step 3.
 * 8) If either Low_Side or High_Side value is greater than the 
 *    alarm threshold, assert the Ground Fault Alarm.
 * 9) report Low_Side and High_Side values, and status of GF Alarm.
 * 10) repeat.
 *
 * [2] Voltage to ground fault current transfer function:
 * 
 * The ADCs on the MSP430 appear to hit full scale (4095 counts) when 2.9VDC is
 * applied. The circuit for ground fault current measurement was designed so 5mV 
 * is produced for every uA of current. The transfer function, therefore, is:
 *
 *     2.9V           1uA GF current
 * --------------- * ---------------- = 0.1416015625 uA GF current/ADC count
 * 4096 ADC counts        0.005 V
 *
 */

int getLowsideExt(float *gfiLoResult);
int getHisideExt(float *gfiHiResult);
int sampleLowside(float *gfiLoResult, unsigned int *pAdcCount);
int sampleHighside(float *gfiHiResult, unsigned int *pAdcCount);

// for interactive polling of ground fault current
int setPeriodHandler(char *cmdStr);
int setSettleHandler(char *cmdStr);
int gfiShowHandler(char *cmdStr);
int gfScanOnHandler(char *cmdStr);
int gfScanOffHandler(char *cmdStr);

void gfiPollHandler(void);
void gfiPoll0Handler(void);
void gfiPoll1Handler(void);

static pTimer pollTimer = NULL; // reference to our periodic timer
static pTimer timer0 = NULL;
static pTimer timer1 = NULL;

static int period = 60; // default period (60 seconds)
static int settleTime = 10; // default settle time (10 seconds)
static int sampleCycleInProgress;
static int gfScanEnable;

static float lowsideValue[2] = {0.0, 0.0};
static float highsideValue[2] = {0.0, 0.0};

// cache results to return if not polled before a new cycle begins
static float lastLowsideResult = 0.0;
static float lastHighsideResult = 0.0;

int gfiInit(void)
{
  sensorRegisterSampleMethod(SENSOR_ID_GFLOWSIDE, 0, getLowsideExt);
  sensorRegisterSampleMethod(SENSOR_ID_GFHIGHSIDE, 0, getHisideExt);
  
  cmdparseAddCommand("set", "gfsettle", setSettleHandler);
  cmdparseAddCommand("set", "gfperiod", setPeriodHandler);
  cmdparseAddCommand("show", "gf", gfiShowHandler);
  cmdparseAddCommand("gfscan", "on", gfScanOnHandler);
  cmdparseAddCommand("gfscan", "off", gfScanOffHandler);

  eventqRegisterHandler(EVENTQ_EVENT_GFIPOLL, gfiPollHandler);
  eventqRegisterHandler(EVENTQ_EVENT_GFIPOLL0, gfiPoll0Handler);
  eventqRegisterHandler(EVENTQ_EVENT_GFIPOLL1, gfiPoll1Handler);


  pollTimer = timerCreate(TIMER_FLAGS_PERIODIC,
                          period,
			  EVENTQ_EVENT_GFIPOLL);			  

  gfScanEnable = true;
  sampleCycleInProgress = false;
  
  return 0;
}

static void gfiPollHandler(void)
{
  // printf("gfipoll (check highside=1)\n");
  if ((sampleCycleInProgress == false) && (gfScanEnable == true)) {
    // initialize state for a GFI sample cycle
    sampleCycleInProgress = true;

    // initialize check highside to true
    gpioSetHiSide(true);

    // queue a timer for the next step in the process
    timer0 = timerCreate(TIMER_FLAGS_ONESHOT, settleTime, EVENTQ_EVENT_GFIPOLL0);
    }
}

static void gfiPoll0Handler(void)
{
  unsigned int loCount, hiCount;
  
  timer0 = NULL;
  
  // sample ADCs to get a baseline
  sampleLowside(&lowsideValue[0], &loCount);
  sampleHighside(&highsideValue[0], &hiCount);

  // printf("gfipoll0: lowside[%f] highside[%f]\n",lowsideValue[0],highsideValue[0]);
  // printf("gfipoll0: check highside = 0\n");
  // set check highside to false
  gpioSetHiSide(false);

  // queue a timer for the final step in the process
  timer1 = timerCreate(TIMER_FLAGS_ONESHOT, settleTime, EVENTQ_EVENT_GFIPOLL1);
}

static void gfiPoll1Handler(void)
{
  unsigned int loCount, hiCount;
  
  timer1 = NULL;

  // sample ADC
  sampleLowside(&lowsideValue[1], &loCount);
  sampleHighside(&highsideValue[1], &hiCount);
  // printf("gfipoll1: lowside[%f] highside[%f]\n",lowsideValue[1],highsideValue[1]);

  lastLowsideResult = lowsideValue[1] - lowsideValue[0];
  lastHighsideResult = highsideValue[1] - highsideValue[0];
  // printf("\ngfi: low[%f] high[%f]\n\n",lastLowsideResult,lastHighsideResult);
  sampleCycleInProgress = false;
}

boolean gfiGetCycleInProgress(void)
{
  return sampleCycleInProgress;
}

static int setPeriodHandler(char *cmdStr)
{
  int retVal, setPeriod;
  
  eatWhiteSpace(&cmdStr);
  retVal = sscanf(cmdStr, "%d", &setPeriod);
  
  if (retVal != 1)
    return ERRNUM_INVALID_ARG;

  // max 1/1sec sampling rate
  
  if (setPeriod < 0) {
    return ERRNUM_INVALID_ARG;
    }
  else if (setPeriod == 0) {
    // cancel periodic timer, but let the current sample cycle finish if in progress
    timerCancel(pollTimer);
    }
  else {
   
    // printf("setPeriodHandler: setting GF poll period to [%d]\n", setPeriod);

    // cancel the two one-shot timers

    if (timer0 != NULL) {
      timerCancel(timer0);
      }
      
    if (timer1 != NULL) {
      timerCancel(timer1);
      }

    // mod timer
    timerModify(pollTimer, TIMER_FLAGS_PERIODIC, setPeriod);
    sampleCycleInProgress = false;   
    }

  period = setPeriod;
  return 0;
}

static int setSettleHandler(char *cmdStr)
{
  int retVal, setSettle;
  
  eatWhiteSpace(&cmdStr);
  retVal = sscanf(cmdStr, "%d", &setSettle);
  
  if (retVal != 1)
    return ERRNUM_INVALID_ARG;

  if (setSettle < 0) {
    return ERRNUM_INVALID_ARG;
    }
  else if (setSettle == 0) {
    // TODO: do we want to allow zero settle time?
    return ERRNUM_INVALID_ARG;
    }

  // cancel current settle time timers??
  settleTime = setSettle;

  return 0;
}

static int getLowsideExt(float *gfiLoResult) 
{
  if (gfScanEnable == true)
    *gfiLoResult = lastLowsideResult;
  else
    *gfiLoResult = -1.0;
    
  return 0;
}

static int getHisideExt(float *gfiHiResult) 
{

  if (gfScanEnable == true)
    *gfiHiResult = lastHighsideResult;
  else
    *gfiHiResult = -1.0;

  return 0;
}


static void convertHighsideCurrent(unsigned int adcValue, float * currentValue)
{
#ifdef CONFIG_GFI_USE_HIGHSIDE_LINEAR
  *currentValue = ((float)adcValue * 0.137249) + 0.562355;
#else
  *currentValue = (float)(adcValue * 2.9)/(4096.0 * 0.005);
#endif
}

static void convertLowsideCurrent(unsigned int adcValue, float * currentValue)
{
#ifdef CONFIG_GFI_USE_LOWSIDE_LINEAR
  *currentValue = ((float)adcValue * 0.125102) + 0.401291;
#else
  *currentValue = (float)(adcValue * 2.9)/(4096.0 * 0.005);
#endif 
}

static int sampleLowside(float *gfiLoResult, unsigned int *pAdcCount)
{
  int retVal = -1;
  unsigned int lowSideResult;
  
  if (adc12GetGfiLo(&lowSideResult) == 0) {
    convertLowsideCurrent(lowSideResult, gfiLoResult);
    *pAdcCount = lowSideResult;
    retVal = 0;
    }

  return retVal;
} 

static int sampleHighside(float *gfiHiResult, unsigned int *pAdcCount)
{
  int retVal = -1;
  unsigned int hiSideResult;
  
  if (adc12GetGfiHi(&hiSideResult) == 0) {
    convertHighsideCurrent(hiSideResult, gfiHiResult);
    *pAdcCount = hiSideResult;
    retVal = 0;
    }

  return retVal;
}

static float showHiside, showLowside;
static unsigned int rawAdcHiside, rawAdcLowside;

static int gfiShowHandler(char *cmdStr)
{
  char *pBuf;
  int bufSize;
  volatile int i;
  
  if (outfmtGetBuffer(&pBuf,&bufSize) != 0)
    return -1;
    
  bufSize--; // leave room for null terminator

  showHiside  = 0.0;
  showLowside = 0.0;
  rawAdcHiside  = 0;
  rawAdcLowside = 0;

  if (gfScanEnable == true) {
    sprintf(pBuf, "\show gfi not available when GF scan enabled!\n\x00");
    outfmtSendBuffer();
    }
  else {
    if (sampleHighside(&showHiside, &rawAdcHiside) == 0)
      sprintf(pBuf, "\nHigh side analog: [%f]  ADC[%u]\n\x00", showHiside, rawAdcHiside);
    else
      sprintf(pBuf, "\nHigh side analog: conversion error\n\x00");
  
    outfmtSendBuffer();

    if (sampleLowside(&showLowside, &rawAdcLowside) == 0)
      sprintf(pBuf, "\nLow  side analog: [%f]  ADC[%u]\n\x00", showLowside, rawAdcLowside);
    else
      sprintf(pBuf, "\nLow  side analog: conversion error\n\x00");
  
    outfmtSendBuffer();
    }
    
  return 0;
}

static int gfScanOnHandler(char *cmdStr)
{
  gfScanEnable = true;
  
  pollTimer = timerCreate(TIMER_FLAGS_PERIODIC,
                          period,
			  EVENTQ_EVENT_GFIPOLL);			  

  gfScanEnable = true;
  sampleCycleInProgress = false;
  
  return 0;
}

static int gfScanOffHandler(char *cmdStr)
{

  if (timer0 != NULL) {
    timerCancel(timer0);
    }
      
  if (timer1 != NULL) {
    timerCancel(timer1);
    }

  gfScanEnable = false;
  sampleCycleInProgress = false;

    
  return 0;
}

#endif