#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 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 getData(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 *);
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, getData);
  
  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);

  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 getData(void) 
{
  int i;
  sensorState * pSensor;
  
  for(i=0;i<SENSOR_NUM_SENSORS;i++)
  {
    pSensor = (sensorState *)(pSensors[i]);
    if (pSensor == NULL)
      continue;
    
    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;
    } 
  }

}

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;
}