#include <msp430x14x.h>
#include <stdio.h>
#include <stdlib.h>
#include <limits.h>

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

#ifdef CONFIG_INCLUDE_KVH

// command handlers
int kvhGetHandler(char *cmdStr);
int kvhPutHandler(char *cmdStr);
int kvhAnalogHandler(char *cmdStr);
int kvhTurnsCounterSetHandler(char *cmdStr);

// event handlers
void kvhReceiveHandler(void);
void kvhRawAdcInHandler(void);

// sensor interface routines
int kvhGetHeading(float * pHeading);
int kvhGetTurnsCounter(float *pTurns);

static float currentHeading;
static float previousHeading;

static int clockwiseTurns;
static int counterClockwiseTurns;
static int lastNetTurns;
static int netTurns;

static int initialHighCount;
static int initialLowCount;

static int lowTurnThresholdCount;
static int highTurnThresholdCount;

static float lowTurnThresholdLevel;
static float highTurnThresholdLevel;

static unsigned int lastRawAdcSample;
static unsigned int maxRawAdcSample;
static unsigned int minRawAdcSample;

static char * haltOutputStr = "h\n";
static char * analogLinearOutStr = "=at,0\n";

static float adcCountsToMvScale = (3300.0 / 4096.0);

/*
 * For the normal linear output mode of the C100 compass, it generates a 
 * sawtooth-like function with zero heading being at 100mV, 180 degrees
 * of heading being at 1000 mV and 360 degrees of heading (i.e. zero degrees + 1 
 * clockwise turn) at 1900 mV. A simple linear representation of this is
 * Vout = 5 * h + 100, where Vout is the linear output in mVDC. Solving for 
 * heading as a function of Vout, we get h = Vout/5 - 20. 
 * 
 * To get the transfer function in ADC counts, we have to quantify the relation
 * between MSP430 ADC12 counts and input voltage, which should in theory be a
 * simple proportional relation. The MSP430 allows for many reference voltages
 * to be selected for each ADC channel. 
 *
 */
 
static void initVars(void)
{
  currentHeading = -1.0;
  previousHeading = -1.0;
  lowTurnThresholdCount = 0;
  highTurnThresholdCount = 0;
  lowTurnThresholdLevel = CONFIG_KVH_LOW_TURN_THRESHOLD_LEVEL;
  highTurnThresholdLevel = CONFIG_KVH_HIGH_TURN_THRESHOLD_LEVEL;
  clockwiseTurns = 0;
  counterClockwiseTurns = 0;
  lastNetTurns = 0;
  netTurns = 0;
  initialLowCount = CONFIG_KVH_LOW_TURN_THRESHOLD_COUNT;
  initialHighCount = CONFIG_KVH_HIGH_TURN_THRESHOLD_COUNT;
  
  minRawAdcSample = 65535U;
  maxRawAdcSample = 0;
}

static void initHardware(void)
{
  gpioSetKvhPower(true);
}

void kvhInit(void)
{
  initVars();
  
  initHardware();
  
  cmdparseAddCommand("kvh", "get", kvhGetHandler);
  cmdparseAddCommand("kvh", "put", kvhPutHandler);
  cmdparseAddCommand("kvh", "analog", kvhAnalogHandler);
  cmdparseAddCommand("set", "turns",  kvhTurnsCounterSetHandler);
    
  sensorRegisterSampleMethod(SENSOR_ID_HEADING,
                             0,
                             kvhGetHeading);

  sensorRegisterSampleMethod(SENSOR_ID_TURNS_COUNT,
                             0,
                             kvhGetTurnsCounter);

  eventqRegisterHandler(EVENTQ_EVENT_UART1RX, kvhReceiveHandler);

  eventqRegisterHandler(EVENTQ_EVENT_ADC12_KVH, kvhRawAdcInHandler);

}

int kvhGetHeading(float * pHeading)
{
  if (pHeading == NULL)
    return -1;
    
  *pHeading = currentHeading;
  return 0;
}

int kvhGetTurnsCounter(float *pTurns)
{
  if (pTurns == NULL)
    return -1;
    
  *pTurns = ((float)netTurns);
  return 0;
}


//
// event handlers
//

void kvhReceiveHandler(void)
{
}

#warning "AQ430-specific inline syntax"


static __inline__ int convertAnalog(unsigned int adcCounts, float *pDegrees)
{
  float tmpHead;
  
  if (pDegrees == NULL)
    return -1;

  /*
   * return -1 if the ADC counts are out of spec for linear mode (100mV <= x <= 1900mV) 
   * example: CONFIG_KVH_ADC_COUNTS_FLOOR = 124
   *          CONFIG_KVH_ADC_COUNTS_CEILING = 2358
   */

  if (adcCounts < CONFIG_KVH_ADC_COUNTS_FLOOR) 
    return -1;
    
  if (adcCounts > CONFIG_KVH_ADC_COUNTS_CEILING)
    return -1;
    
  tmpHead = ( ( ( ((float)adcCounts) * adcCountsToMvScale) - 100.0) / 5.0);

#ifdef CONFIG_KVH_ZERO_CLIP
  if (tmpHead < 0.0) {
    tmpHead = 0.0;
    }

  if (tmpHead > 359.9) {
    tmpHead = 359.9;
    }
#endif

#ifdef CONFIG_KVH_ZERO_WRAP
  if (tmpHead < 0.0) {
    tmpHead += 359.9;
    }

  if (tmpHead > 359.9) {
    tmpHead -= 359.9;
    }
#endif

  *pDegrees = tmpHead;
     
  return 0;
}

static __inline__ void updateHeading(float newHeading)
{
  previousHeading = currentHeading;
  currentHeading = newHeading;

  if (currentHeading >= highTurnThresholdLevel) {
    if (lowTurnThresholdCount > 0) {
      counterClockwiseTurns++;
      lowTurnThresholdCount = 0;
      }
    highTurnThresholdCount = initialHighCount;
    }
  else if (currentHeading <= lowTurnThresholdLevel) {
    if (highTurnThresholdCount > 0) {
      clockwiseTurns++;
      highTurnThresholdCount = 0;
      }
    lowTurnThresholdCount = initialLowCount;
    }
  else {
   if (highTurnThresholdCount > 0) highTurnThresholdCount--;
   if (lowTurnThresholdCount > 0) lowTurnThresholdCount--;
   }
  
  netTurns = clockwiseTurns - counterClockwiseTurns;
  
  // raise an event if the turns count has changed to update the reported data
  if (lastNetTurns != netTurns) {
    eventqPut(EVENTQ_EVENT_UPDATE_TC);
    }
    
  lastNetTurns = netTurns;
}

void kvhRawAdcInHandler(void)
{
  float newHeading;
  unsigned int rawSample;
  
  // get the data from the adc12 module  
  while (adc12GetKvh(&rawSample) != -1) {
    lastRawAdcSample = rawSample;
    if (rawSample > maxRawAdcSample)
      maxRawAdcSample = rawSample;
    if (rawSample < minRawAdcSample)
      minRawAdcSample = rawSample;
    if (convertAnalog(rawSample, &newHeading) != -1) {
      updateHeading(newHeading);
      }
    }

  eventqPut(EVENTQ_EVENT_NEW_HEADING);
}

int kvhTurnsCounterSetHandler(char *cmdStr)
{
  int retVal, setTurns;
  
  eatWhiteSpace(&cmdStr);
  retVal = sscanf(cmdStr, "%d", &setTurns);
  
  if (retVal != 1) {
    return ERRNUM_INVALID_ARG;
    }

  if (setTurns < 0) {
    counterClockwiseTurns = abs(setTurns);  
    clockwiseTurns = 0;
    }
  else {
    clockwiseTurns = setTurns;  
    counterClockwiseTurns = 0;
    }

  netTurns = clockwiseTurns - counterClockwiseTurns;

  eventqPut(EVENTQ_EVENT_UPDATE_TC);
  
  return 0;  
}


int kvhPutHandler(char *cmdStr)
{
  char ch;
  
  eatWhiteSpace(&cmdStr);
  
  while (*cmdStr != 0x00) {
    ch = *cmdStr++;
    while (usart1TxOne(ch) != 0);
    }
    
  usart1TxOne(0x0d); // send CR
    
  return 0;
}

int kvhGetHandler(char *cmdStr)
{
  char ch;
  char *pBuf;
  int bufSize;
  int bufIdx;
  
  bufIdx = 0;
  if (outfmtGetBuffer(&pBuf,&bufSize) != 0)
    return -1;
    
  bufSize--; // leave room for null terminator
  
  while ((usart1RxOne(&ch) == 0) && (bufIdx < bufSize))
    pBuf[bufIdx++] = ch;
    
  pBuf[bufIdx] = 0x00;
  
  outfmtSendBuffer();
  
  return 0;
}

int kvhAnalogHandler(char *cmdStr)
{
  char *pBuf;
  int bufSize;
  unsigned int temp = 0;
  float kvhResult;
  
  if (outfmtGetBuffer(&pBuf,&bufSize) != 0)
    return -1;
    
  bufSize--; // leave room for null terminator

  convertAnalog(lastRawAdcSample, &kvhResult);
  sprintf(pBuf, "\nKVH Hdg[%4.2f] ADC: cur[%u] min[%u] max[%u]\n\x00", 
    kvhResult, lastRawAdcSample, minRawAdcSample, maxRawAdcSample);
  
  outfmtSendBuffer();

  // reset min and max
  minRawAdcSample = 65535U;
  maxRawAdcSample = 0;
  
  return 0;
}

#endif /* CONFIG_INCLUDE_KVH */