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

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

static boolean initDone = false;
static boolean turnStreamEnable = false;

static int alarmCount = 0;

// keep an array of ptrs to sensorState structures
static sensorState * pSensors[SENSOR_NUM_SENSORS];

#ifdef CONFIG_NO_MALLOC
static sensorState sensors[SENSOR_NUM_SENSORS];
static int freeIdx = 0;
#endif

// local functions for event handler
void updateSensorAll(void);
void updateSensorTurnsCount(void);
void updateSensorHeading(void);

// local function protos for cmd handler registration
int setAmaskHandler(char *);
int setLevelsHandler(char *);
int getAmaskHandler(char *);
int getAlarmHandler(char *);
int getLevelsHandler(char *);
int clearAlarmHandler(char *);
int getDataHandler(char *);
int turnsStreamEnableHandler(char *);
int turnsStreamDisableHandler(char *);

static unsigned char getAlarms(void);

static sensorState * allocSensor(void) 
{
  sensorState * pRetVal = NULL;

#ifdef CONFIG_NO_MALLOC
  if (freeIdx < SENSOR_NUM_SENSORS) {
    pRetVal = (sensorState *)(&sensors[freeIdx++]);
  }
#else
  pRetVal = malloc(sizeof(sensorState));
#endif

  return pRetVal;
}

static void updateState(sensorState *pSensor, unsigned char newState)
{
  // shadow the variables for convenience
  unsigned char prevState;
  unsigned char curState;
  
  if (pSensor == NULL)
    return;
  
  prevState = pSensor->curState;
  curState = newState;
  
  pSensor->prevState = prevState;
  pSensor->curState = curState;

  // check state transitions and update global alarm count
  if (curState != prevState) {
  	if (curState == SENSOR_STATE_TRIGGERED) {
  	  alarmCount++;
  	  }
  	else if (prevState == SENSOR_STATE_TRIGGERED) {
  	  alarmCount--;
  	  }
  	}  
    
  // update the SideARM attention signal
  if (alarmCount > 0)
    gpioSetAttn(true);
  else
    gpioSetAttn(false);
  
}

int sensorInit(void)
{
  // register this module as the recipient of the 1/s watchdog timer event
  eventqRegisterHandler(EVENTQ_EVENT_DATAPOLL, updateSensorAll);
  eventqRegisterHandler(EVENTQ_EVENT_UPDATE_TC, updateSensorTurnsCount);
  eventqRegisterHandler(EVENTQ_EVENT_NEW_HEADING, updateSensorHeading);
  
  cmdparseAddCommand("set", "amask", setAmaskHandler);
  cmdparseAddCommand("get", "amask", getAmaskHandler);

  cmdparseAddCommand("set", "levels", setLevelsHandler);
  cmdparseAddCommand("get", "levels", getLevelsHandler);

  cmdparseAddCommand("clear", "alarm", clearAlarmHandler);
  cmdparseAddCommand("get", "alarm", getAlarmHandler);
  
  cmdparseAddCommand("get", "data", getDataHandler);
  
  cmdparseAddCommand("loudturn","on", turnsStreamEnableHandler);
  cmdparseAddCommand("loudturn","off", turnsStreamDisableHandler);

  turnStreamEnable = false;
  initDone = true;  
  return 0;
}

// TODO: change to 'sensorRegisterSensor()'
int sensorRegisterSampleMethod(int sensorId, unsigned char flags, sensorSampleMethod method)
{
  sensorState * pNewSensor;
  
  if (initDone == false)
  	return -1;
  	
  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS))
    return -1;
  	
  pNewSensor = (sensorState *)allocSensor();
   
  if (pNewSensor == NULL) {
  	return -1; // TODO: error value
  	}
    
  pNewSensor->levelHi = 0.0;
  pNewSensor->levelLo = 0.0;
  pNewSensor->lastValue = 0.0;
  pNewSensor->flags = (flags & SENSOR_FLAGS_USERMASK);    
  pNewSensor->method = method;

  updateState(pNewSensor, SENSOR_STATE_INITIAL);

  pSensors[sensorId] = pNewSensor;
  
  return 0;
}

int sensorGetData(int sensorId, float * pValue)
{
  sensorState * pSensor;
  
  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS))
    return -1;
  
  pSensor = (sensorState *)(pSensors[sensorId]);
  if (pSensor == NULL) {
    return -1;
    }

  *pValue = pSensor->lastValue;  
  return 0;
}

// levelId is (sensorId << 1) and ORed with 0x1 
// if high level and 0x0 if low level

int sensorGetLevel(int levelId, float *pValue)
{
  sensorState * pSensor;
  int sensorId = (levelId >> 1);

  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS)) {
    return -1;
    }

  pSensor = (sensorState *)(pSensors[sensorId]);
  if (pSensor == NULL) {
  	return -1;
  	}
  	
  if ((levelId & 0x1) == 0)
    *pValue = pSensor->levelLo;
  else
    *pValue = pSensor->levelHi;

  return 0;
}

int sensorSetLevel(int levelId, float value)
{
  sensorState * pSensor;
  int sensorId = (levelId >> 1);
  
  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS)) {
    return -1;
    }
  
  pSensor = (sensorState *)(pSensors[sensorId]);
  if (pSensor == NULL) {
  	return -1;
  	}
  
  if ((levelId & 0x1) == 0)
    pSensor->levelLo = value;
  else
    pSensor->levelHi = value;
  
  if (pSensor->levelLo < pSensor->levelHi)
    pSensor->flags |= SENSOR_FLAGS_LEVELSOK;
  else
    pSensor->flags &= ~SENSOR_FLAGS_LEVELSOK;
  
  return 0;
}

static int getDataHandler(char * cmdStr)
{
  outfmtSendData();
  return 0;
}

static void updateSensor(int sensorId)
{
  sensorState * pSensor;

  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS))
    return;
    
  pSensor = (sensorState *)(pSensors[sensorId]);
  if (pSensor == NULL)
    return;
    
  switch(pSensor->curState) {
    case SENSOR_STATE_UNARMED:
      // get the data
      if (pSensor->method != NULL) {
	    // printf("getData: sensor[%d] sampled (unarmed)\n",i);
        (*pSensor->method)(&(pSensor->lastValue));
        }
      break;
    case SENSOR_STATE_ARMED:
      // get the data
      if (pSensor->method != NULL) {
	    // printf("getData: sensor[%d] sampled (armed)\n",i);
        (*pSensor->method)(&(pSensor->lastValue));
	    }
	
      // check the values
      if ((pSensor->flags & SENSOR_FLAGS_LEVELSOK) != 0) {
        if ((pSensor->lastValue < pSensor->levelLo) || 
            (pSensor->lastValue > pSensor->levelHi)) {
            // newly triggered
            updateState(pSensor, SENSOR_STATE_TRIGGERED);
            }
        }
        break;
      case SENSOR_STATE_TRIGGERED:
	    // printf("getData: sensor[%d] still triggered\n",i);
        // still triggered
        break;
      default:
        // printf("getData: unknown state encountered: [%d]\n",pSensor->curState);
        break;
    } // case
  }

static void updateSensorAll(void) 
{
  int i;
  
  for(i=0; i<SENSOR_NUM_SENSORS; i++) {
    updateSensor(i);
    }
}

static void updateSensorTurnsCount(void)
{
  updateSensor(SENSOR_ID_TURNS_COUNT);
  if (turnStreamEnable == true) {
    outfmtSendData();
    }  
}

static void updateSensorHeading(void)
{
  updateSensor(SENSOR_ID_HEADING);
}

int sensorTriggerAlarm(int sensorId)
{
  sensorState * pSensor;
  
  if ((sensorId < 0) || (sensorId >= SENSOR_NUM_SENSORS))
    return -1;
    
  pSensor = (sensorState *)(pSensors[sensorId]);
  if (pSensor == NULL)
    return -1;
    
  if (pSensor->curState == SENSOR_STATE_ARMED) {
    updateState(pSensor, SENSOR_STATE_TRIGGERED);  
    }
  
  return 0;
}

static unsigned char getAlarms(void)
{
  int i;
  unsigned char mask = 0x1;
  unsigned char retVal = 0x00;
  sensorState * pSensor;
  
  for (i=0; i<SENSOR_NUM_SENSORS; i++, mask <<= 1) 
  {
    pSensor = pSensors[i];
    if (pSensor == NULL)
      continue;
    if (pSensor->curState == SENSOR_STATE_TRIGGERED)
      retVal |= mask;
  }
  return retVal;
}

static void clearAlarms(unsigned char clrMask)
{
  int i;
  unsigned char mask = 0x1;
  sensorState * pSensor;
  
  for (i=0; i<SENSOR_NUM_SENSORS; i++, mask <<= 1) {
    pSensor = pSensors[i];
    if (pSensor == NULL)
      continue;
      
    if ((clrMask & mask) != 0) {
      switch(pSensor->curState) {
        case SENSOR_STATE_UNARMED:
        case SENSOR_STATE_ARMED:
          // noop
          break;
        case SENSOR_STATE_TRIGGERED:
          updateState(pSensor, SENSOR_STATE_ARMED);
          break;
        default:
          break;
      } // switch(curState)
    } // if
  } // for
} // clearAlarms()

static unsigned char getAmask(void)
{
  int i;
  unsigned char mask = 0x1;
  unsigned char retVal = 0x00;
  sensorState * pSensor;
  
  for (i=0; i<SENSOR_NUM_SENSORS;i++, mask <<= 1)  {
    pSensor = pSensors[i];
    if (pSensor == NULL)
      continue;
    if (pSensor->curState != SENSOR_STATE_UNARMED)
      retVal |= mask;
    }
  return retVal;
}

static void setAmask(unsigned char setMask)
{
  int i;
  unsigned char mask = 0x1;
  sensorState * pSensor;
  
  for (i=0; i<SENSOR_NUM_SENSORS;i++,mask <<= 1) {
    pSensor = pSensors[i];
    if (pSensor == NULL)
      continue;
    switch(pSensor->curState) {
      case SENSOR_STATE_UNARMED:
        if ((setMask & mask) != 0)
          updateState(pSensor, SENSOR_STATE_ARMED);
        break;
      case SENSOR_STATE_ARMED:
        if ((setMask & mask) == 0)
          updateState(pSensor, SENSOR_STATE_UNARMED);
        break;
      case SENSOR_STATE_TRIGGERED:
      default:
        break;
      } // switch(curState)
    } // for
    
}

static int setAmaskHandler(char * cmdStr)
{
  int retVal, aMask;
  
  eatWhiteSpace(&cmdStr);
  retVal = sscanf(cmdStr, "%x",&aMask);

  if (retVal != 1)
    return ERRNUM_INVALID_ARG;
  else 
    setAmask((unsigned char)(aMask & 0xff));

  return 0;
}

static int getAmaskHandler(char * cmdStr)
{
  unsigned char amask = getAmask();
  outfmtSendAlarmInfo(0, amask);
  return 0;
}

static int setLevelsHandler(char * cmdStr)
{
  eatWhiteSpace(&cmdStr);
  return levparseSetLevels(cmdStr);
}

static int getLevelsHandler(char * cmdStr)
{
  outfmtSendLevels();
  return 0;
}

static int clearAlarmHandler(char * cmdStr)
{
  int retVal, clrMask;

  eatWhiteSpace(&cmdStr);
  retVal = sscanf(cmdStr, "%x",&clrMask);

  if (retVal != 1)
    return ERRNUM_INVALID_ARG;
  else
    clearAlarms((unsigned char)(clrMask & 0xff));
    
  return 0;
}

static int getAlarmHandler(char * cmdStr)
{
  unsigned char alarms = getAlarms();
  outfmtSendAlarmInfo(1, alarms);
  return 0;
}

static int turnsStreamEnableHandler(char * cmdStr)
{
  turnStreamEnable = true;
  return 0;
}

static int turnsStreamDisableHandler(char * cmdStr)
{
  turnStreamEnable = false;
  return 0;
}