/*
 * CommandHandler.cpp
 *
 *  Created on: Nov 29, 2017
 *      Author: Bmiller
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*
* DESCRIPTION:  The routines in this module handle commands from the Serial and Bluetooth interfaces.
*
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*   Copyright (c) 2010 - 2018  SRD / Metrohm Raman
*   THE INFORMATION CONTAINED IN THIS file IS THE sole intellectual property OF
*   SNOWY RANGE INSTRUMENTS and protected under copyright law. ANY REPRODUCTION
*   or dissemination IN PART OR WHOLE WITHOUT THE express WRITTEN PERMISSION OF
*   SNOWY RANGE INSTRUMENTS IS PROHIBITED.
* +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 */
#include "CommandHandler.h"

#include <ctype.h>
#include <strings.h>
#include <stdio.h>
#include <stdlib.h>

#include "sdi_task.h"
#include <time.h>
#include <ti/sysbios/hal/Seconds.h>
#include <ti/sysbios/knl/Task.h>
//#include <sys_ctrl.h>

#include "SRI_Global.h"
#include "ASCII.h"
#include "util.h"
#include "Parse.h"
#include "TargetGlobal.h"
#include "EEPROM.h"
#include "Raster.h"
#include "Laser.h"
#include "IMX290.h"

// Needed for HAL_SYSTEM_RESET()
#include "hal_mcu.h"
#include <inc/hw_types.h>

static char buffer[64] = {0};


/*void ADread_handler(void)
{

}*/

/*void averaging_handler(void)
{
    unsigned int avgstmp;

      if (parsed[parsed_tail].token == SET)
      {
        sscanf(parsed[parsed_tail].string[0], "%d", &avgstmp);

        if ((avgstmp > parsed[parsed_tail].limits[0].upper) || (avgstmp < parsed[parsed_tail].limits[0].lower))
        {
            SDITask_Printf("\nAVERAGING: ERROR, NEW VALUE OUT OF RANGE\n");
            return;
        }
        else
        {
            status.Averages = avgstmp;
            //SDITask_Printf("\n Averages set: %d", status.Averages);
        }
      }

      if ((status.verbose == ON) || (parsed[parsed_tail].token == QUERY))
      {
          SDITask_Printf("\n AVERAGES: %d\n DEFAULT AVERAGES: %d", status.Averages, eeprom_base.eeprom.Averages);
      }
      else
      {
      }
}*/


void cal_handler(void)
{
    if (parsed[parsed_tail].token == SET)
    {
        eeprom_base.eeprom.CalP0 = strtod(parsed[parsed_tail].string[1], NULL);
        eeprom_base.eeprom.CalP1 = strtod(parsed[parsed_tail].string[2], NULL);
        eeprom_base.eeprom.CalP2 = strtod(parsed[parsed_tail].string[3], NULL);
        eeprom_base.eeprom.CalP3 = strtod(parsed[parsed_tail].string[4], NULL);
        eeprom_base.eeprom.CalP4 = strtod(parsed[parsed_tail].string[5], NULL);
        EEPROM_Write();
        //uint32_t time;
        //UTC_init();
        //time = UTC_getClock();
        //sprintf(buffer,"%d[%2]/%d[%2]/%d[%4] %d[%2]:%d[%2]");
        //eeprom_base.eeprom.CalDate = buffer;
        if(status.Software == OFF)
        {
            SDITask_Printf("\n Success Cal Params Set." );
        }

    }
    else if (parsed[parsed_tail].token == DATETIME)
    {
        SDITask_Printf("\n Cal Date: %s.", eeprom_base.eeprom.CalDate);
    }
    else
    {
        dtoa(eeprom_base.eeprom.CalP0,buffer,14);
        SDITask_Printf("\n Cal Params P0: %s",buffer );
        dtoa(eeprom_base.eeprom.CalP1,buffer,14);
        SDITask_Printf("\n P1: %s", buffer);
        dtoa(eeprom_base.eeprom.CalP2,buffer,14);
        SDITask_Printf("\n P2: %s", buffer);
        dtoa(eeprom_base.eeprom.CalP3,buffer,14);
        SDITask_Printf("\n P3: %s", buffer);
        dtoa(eeprom_base.eeprom.CalP4,buffer,14);
        SDITask_Printf("\n P4: %s", buffer);
    }
}

void ycal_handler(void)
{
    if (parsed[parsed_tail].token == SET)
    {
        if(parsed[parsed_tail].string[5] != NULL)
        {
            eeprom_base.eeprom.YCalP0 = strtod(parsed[parsed_tail].string[1], NULL);
            eeprom_base.eeprom.YCalP1 = strtod(parsed[parsed_tail].string[2], NULL);
            eeprom_base.eeprom.YCalP2 = strtod(parsed[parsed_tail].string[3], NULL);
            eeprom_base.eeprom.YCalP3 = strtod(parsed[parsed_tail].string[4], NULL);
            eeprom_base.eeprom.YCalP4 = strtod(parsed[parsed_tail].string[5], NULL);
            EEPROM_Write();
        }
        else
        {
            SDITask_Printf("\n Fail setting YCal Params. Must set 5 Params. Ex: Ycal params 1 2 3 4 5" );
        }

        //uint32_t time;
        //UTC_init();
        //time = UTC_getClock();
        //sprintf(buffer,"%d[%2]/%d[%2]/%d[%4] %d[%2]:%d[%2]");
        //eeprom_base.eeprom.CalDate = buffer;
        if(status.Software == OFF)
        {
            SDITask_Printf("\n Success YCal Params Set." );
        }
    }
    else if (parsed[parsed_tail].token == DATETIME)
    {
        SDITask_Printf("\n YCal Date: %s.", eeprom_base.eeprom.YCalDate);
    }
    else
    {
        dtoa(eeprom_base.eeprom.YCalP0,buffer,14);
        SDITask_Printf("\n YCal Params P0: %s",buffer );
        dtoa(eeprom_base.eeprom.YCalP1,buffer,14);
        SDITask_Printf("\n P1: %s", buffer);
        dtoa(eeprom_base.eeprom.YCalP2,buffer,14);
        SDITask_Printf("\n P2: %s", buffer);
        dtoa(eeprom_base.eeprom.YCalP3,buffer,14);
        SDITask_Printf("\n P3: %s", buffer);
        dtoa(eeprom_base.eeprom.YCalP4,buffer,14);
        SDITask_Printf("\n P4: %s", buffer);
    }
}

/*void cal_state_handler(void)
{
    //cal_state_cmd();
    SDITask_Printf("\n Hello World \n");//echo
}*/

/*void ccd_handler(void)
{
    //ccd_cmd();
}*/

/*void Imager_Cooling_handler(void)
{
    //Imager_Cooling_cmd();
}*/

void datetime_handler(void)
{
    if (parsed[parsed_tail].token == SET)
      {

      }
    else
    {
        uint32_t sec = Seconds_get();
        SDITask_Printf("\n Ticks: %d",sec);
    }

}

void EEPROM_handler(void)
{
      if (parsed[parsed_tail].token == SET)
      {
          EEPROM_Write();
      }
      else if(parsed[parsed_tail].token == RESET)
      {
          EEPROM_Reset();
      }
      else
      {
          SDITask_Printf("\n EEPROM Use Update or Reset \n");
      }
}


void gauss_handler(void)
{
  float new_gaussian_sigma;
  float new_gaussian_threshold;
  unsigned int  gaussLoop = 0;

  if (parsed[parsed_tail].token == SET)
  {
    new_gaussian_sigma = strtof(parsed[parsed_tail].string[1], NULL);
    new_gaussian_threshold = strtof(parsed[parsed_tail].string[2], NULL);
    if (((new_gaussian_sigma >= 0.0F) && (new_gaussian_sigma <= 10)) && ((new_gaussian_threshold >= 0) && (new_gaussian_threshold < 1)))
    {
      eeprom_base.eeprom.GaussSigma = new_gaussian_sigma;
      eeprom_base.eeprom.GaussThreshold = new_gaussian_threshold;
      EEPROM_Write();
      gen_gauss_coeffs(new_gaussian_sigma, new_gaussian_threshold);
    }
    else
    {
      SDITask_Printf("\nGAUSS: PARAMETER OUT OF RANGE");
    }
  }
  else if ((parsed[parsed_tail].token == QUERY) || (status.verbose == ON))
  {
    dtoa(eeprom_base.eeprom.GaussSigma,buffer,4);
    SDITask_Printf("\nGAUSSIAN SIGMA = %s", buffer);
    dtoa(eeprom_base.eeprom.GaussThreshold,buffer,4);
    SDITask_Printf("   THRESHOLD = %s", buffer);
    while (gaussLoop <= gauss_max_coeff_index)
    {
        dtoa(gauss_coeffs[gaussLoop],buffer,4);
        SDITask_Printf("\n %s",buffer);
        gaussLoop++;
    }
  }
}   /* end Gauss_handler() */


void has_handler(void)
{
  sscanf(parsed[parsed_tail].string[0], "%s", buffer);
  if (parsed[parsed_tail].token == TOK_ON)
  {
      /* What type of processor does the unit have? */
      if (strcasecmp(buffer, "PROCESSOR") == 0)
      {
          //SDITask_Printf("\n Processor: 3");
          SDITask_Printf("\n 3");
      }
      else if (strcasecmp(buffer, "WAVELENGTH") == 0)
      {
          //SDITask_Printf("\n WaveLength: %d", eeprom_base.eeprom.Wavelength);
          SDITask_Printf("\n %d", eeprom_base.eeprom.Wavelength);
      }
      /*What laser wavelength's are availiable on this unit*/
      else if (strcasecmp(buffer, "LASERPWRS") == 0)
      {
          SDITask_Printf("\n%02d", eeprom_base.eeprom.laserpwrs);
      }
  }
  else if (parsed[parsed_tail].token == QUERY)
  {
      SDITask_Printf("\n Processor: 3");
      SDITask_Printf("\n WaveLength: %d", eeprom_base.eeprom.Wavelength);
      SDITask_Printf("\n LaserPower Levels: %02d", eeprom_base.eeprom.laserpwrs);
  }
  /*else if (parsed[parsed_tail].token == SET)
  {
      if (strcasecmp(buffer, "LASERPWRS") == 0)
      {
        if ((subset_cmd > 0) && (subset_cmd < 6))
        {
            eeprom_base.eeprom.laserpwrs = subset_cmd;
        }
        else
        {
            SDITask_Printf("\n Invalid power range, set between 0 and 5");
        }
      }
  }*/
}

/*void illuminator_handler(void)
{
    //illuminator_cmd();
}*/

void imager_handler(void)
{
    uint16_t centerline;
    uint8_t read_value;
    uint16_t reg_addr;
    uint16_t value = 0;

    switch (parsed[parsed_tail].token)
    {
        case ALIGN:
            centerline = atoi(parsed[parsed_tail].string[1]);
            if (centerline > 940)
            {
            SDITask_Printf("\n WINDOW POSITION, %d, OUT OF RANGE 0-940", centerline);
            return;
            }
            eeprom_base.eeprom.ImagerCenterline = centerline;
            EEPROM_Write();
            break;

        case GAIN:
            value = atoi(parsed[parsed_tail].string[1]);
            if (value > 240)
            {
              SDITask_Printf("\n GAIN, %d, OUT OF RANGE 0-240", value);
              return;
            }
            eeprom_base.eeprom.ImagerGain = (uint8_t)value;
            EEPROM_Write();
            break;

        case RANGE:
            value = atoi(parsed[parsed_tail].string[1]);
             if (value > 1)
             {
               SDITask_Printf("\n GAIN SELECT MUST BE 0 OR 1");
               return;
             }
            eeprom_base.eeprom.ImagerGainSelect = (uint8_t)value;
            EEPROM_Write();
            break;

        case OFFSET:
            value = atoi(parsed[parsed_tail].string[1]);
            if (value > 255)
            {
              SDITask_Printf("\n Offset, %d, OUT OF RANGE 0-255", value);
              return;
            }
            eeprom_base.eeprom.ImagerOffset = (uint8_t)value;
            EEPROM_Write();
            break;

        case READ:
            IMX290_Start();
            sscanf(parsed[parsed_tail].string[1], "%x", &reg_addr);
            if ((reg_addr < 0x3000) || (reg_addr > 0x34FF))
            {
              SDITask_Printf("\n REGISTER ADDR, %x, OUT OF RANGE 3000-34FF", reg_addr);
              return;
            }
            read_value = IMX290_Read(reg_addr);
            SDITask_Printf("\nIMX290 REG %x: %x", reg_addr, read_value);
            IMX290_Power(OFF);
            break;

    case BINS:
        value = atoi(parsed[parsed_tail].string[1]);
        if (value > 10)
        {
          SDITask_Printf("\n Bins, %d, OUT OF RANGE 0-10", value);
          return;
        }
        eeprom_base.eeprom.ImagerBins = (uint8_t)value;
        EEPROM_Write();
        break;

    default:
      break;
    }
    /* QUERY */
    SDITask_Printf("\nImager Window Centerline: %d", eeprom_base.eeprom.ImagerCenterline);
    SDITask_Printf("\nImager Gain: %d", eeprom_base.eeprom.ImagerGain);
    SDITask_Printf("\nImager Gain Range: %d", eeprom_base.eeprom.ImagerGainSelect);
    SDITask_Printf("\nImager Offset: %d", eeprom_base.eeprom.ImagerOffset);
    SDITask_Printf("\nImager Bins: %d", eeprom_base.eeprom.ImagerBins);
}

void int_time_handler(void)
{
    float int_time;

      switch (parsed[parsed_tail].token)
      {
      case CHANGE:
        //sscanf(parsed[parsed_tail].string[0], "%f", &int_time);
        int_time = strtof(parsed[parsed_tail].string[0], NULL);
        // Limits are scaled in 10 mS per LSB to get decimal values.
        if ((int_time*100 > parsed[parsed_tail].limits[0].upper) || (((int_time*100)+0.001) < parsed[parsed_tail].limits[0].lower))
        {
            SDITask_Printf("\n ERROR, NEW TIME OUT OF RANGE");
          return;
        }
        else
        {
          status.integration_time = int_time;
        }
        break;
      case SET:
          int_time = strtof(parsed[parsed_tail].string[1], NULL);
       // sscanf(parsed[parsed_tail].string[1], "%f", &int_time);
        // Limits are scaled in 10 mS per LSB to get decimal values.
        if ((int_time*100 > parsed[parsed_tail].limits[0].upper) || (((int_time*100)+0.001) < parsed[parsed_tail].limits[0].lower))
        {
            SDITask_Printf("\n ERROR, NEW TIME OUT OF RANGE");
          return;
        }
        else
        {
          status.integration_time = int_time;
          eeprom_base.eeprom.IntTime = int_time;
          //EEPROM_Write();
        }
        break;
      case AUTO:
        if (strcasecmp(parsed[parsed_tail].string[1], "ON") == 0) status.AutoInt = ON;
        else if (strcasecmp(parsed[parsed_tail].string[1], "OFF") == 0) status.AutoInt = OFF;
        break;
     /* case CAL:
        //sscanf(parsed[parsed_tail].string[1], "%f", &int_time);
          int_time = strtof(parsed[parsed_tail].string[1], NULL);
        if ((int_time > 5) || (int_time < 0.01))
        {
            SDITask_Printf("\n CAL INT_TIME: ERROR, NEW CAL INT TIME, %4.2f, OUT OF RANGE (0.01 - 5.0)", int_time);
          return;
        }
        else
        {
          //eeprom.CalIntTime = int_time;
          //EEPROM_UpdateChecksum();
        }
        break;*/
      case MAX:
        //sscanf(parsed[parsed_tail].string[1], "%f", &int_time);
        int_time = strtof(parsed[parsed_tail].string[1], NULL);
        if ((int_time > 60) || (int_time < 0.05))
        {
            SDITask_Printf("\n MAX INT_TIME: ERROR, NEW TIME OUT OF RANGE");
          return;
        }
        else
        {
          eeprom_base.eeprom.MaxAutoIntTime = int_time;
          EEPROM_Write();
        }
      default:
        break;

      } /* end case parse[parsed_tail].token */

      if ((status.verbose == ON) || (parsed[parsed_tail].token == QUERY))
      {
          dtoa(status.integration_time,buffer,3);
          SDITask_Printf("\n INT_TIME: %s",buffer);
          dtoa(eeprom_base.eeprom.MaxAutoIntTime,buffer,3);
          SDITask_Printf("\n MAX AUTO INT TIME: %s", buffer);
          //SDITask_Printf("\nCAL_INT_TIME: %06.2f SECONDS", eeprom.CalIntTime);
      }
      else
      {
      }
}

void itemp_handler(void)
{
    //itemp_cmd();
    SDITask_Printf("\n Hello World \n");//echo
}

/*void Key_install_handler(void)
{
    //Key_installer_cmd();
}*/

void laserpwr_handler(void)
{
    laserpwr_cmd();
}

void LDNo_handler(void)
{
    //LDNo_cmd();
    SDITask_Printf("\n Hello World \n");//echo
}

void ldtemp_handler(void)
{
    ldtemp_cmd();
}

void Level_handler(void)
{
  unsigned int level_number;
  unsigned int level_tmp;
  unsigned int laser_tmp;
  int i;

  if (parsed[parsed_tail].token == SAVE)
  {
    if (sscanf(parsed[parsed_tail].string[0], "%u", &level_number) == EOF ||
        sscanf(parsed[parsed_tail].string[1], "%u", &level_tmp) == EOF)
    {
        SDITask_Printf("\n LEVEL_CMD: PARAMETER MISSING?");
    }
    /* Perform range checking here, if desired */
    if ((level_number > 5) || (level_number < 1))
    {
        SDITask_Printf("\n LEVEL_CMD: LEVEL NUMBER, %d, OUT OF RANGE (1-5)", level_number);
        return;
    }
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[level_number] = level_tmp;
    //EEPROM_Write();

  } /* end if setting a single level parameter. */
  else if (parsed[parsed_tail].token == LASER)
  {
    sscanf(parsed[parsed_tail].string[1], "%u", &laser_tmp);
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[0] = laser_tmp;
    sscanf(parsed[parsed_tail].string[2], "%u", &laser_tmp);
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[1] = laser_tmp;
    sscanf(parsed[parsed_tail].string[3], "%u", &laser_tmp);
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[2] = laser_tmp;
    sscanf(parsed[parsed_tail].string[4], "%u", &laser_tmp);
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[3] = laser_tmp;
    sscanf(parsed[parsed_tail].string[5], "%u", &laser_tmp);
    if (level_tmp > 900)
    {
        SDITask_Printf("\n LEVEL_CMD: VALUE, %d, OUT OF RANGE (0-900)", level_tmp);
        return;
    }
    eeprom_base.eeprom.LaserPwrLevels[4] = laser_tmp;
    //EEPROM_Write();
  }
  if ((status.verbose == ON) || (parsed[parsed_tail].token == QUERY))
  {
    for (i = 0; i < 5; i++)
    {
        SDITask_Printf("\n %u", eeprom_base.eeprom.LaserPwrLevels[i]);
    }
  }
  else
  {
  }
}

void Offset_handler(void)
{
    //Offset_cmd();
    SDITask_Printf("\n Hello World \n");//echo
}

void Param_handler(void)
{
    //Param_cmd();
    SDITask_Printf("\n Hello World \n");//echo
}

/*void Port_handler(void)
{
    //Port_cmd();
}*/

void Power_handler(void)
{
    //Power_cmd();
    SDITask_Printf("\n Hello World \n");
}

void Range_handler(void)
{
     int start_tmp;
     int end_tmp;

     if (parsed[parsed_tail].token == SET)
     {
       if ((sscanf(parsed[parsed_tail].string[1], "%d", &start_tmp) == EOF) ||
           (sscanf(parsed[parsed_tail].string[2], "%d", &end_tmp) == EOF))
       {
         SDITask_Printf("\nRANGE_CMD: PARAMETER MISSING?");
         return;
       }

       // Perform range checking here, if desired
       if ((start_tmp < 0) || (start_tmp > end_tmp))
       {
         SDITask_Printf("\nRANGE_CMD: START VALUE, %04d, OUT OF RANGE (0..%d)",start_tmp, end_tmp);
         return;
       }
       if ((end_tmp < start_tmp) || (end_tmp > 2301)) //(end_tmp > CCD_MAX_IMAGE_SIZE))
       {
         SDITask_Printf("\nRANGE_CMD: END VALUE, %04d, OUT OF RANGE (%d..%d)", end_tmp, start_tmp, 2301);
         return;
       }
       eeprom_base.eeprom.RangeStart = start_tmp;
       //eeprom_base.eeprom.RangeStart = start_tmp;
       eeprom_base.eeprom.RangeEnd = end_tmp;
       //eeprom_base.eeprom.RangeEnd = end_tmp + 1;
       //DISPLAY_WN_SIZE = (eeprom.ImageEnd - eeprom.ImageStart);
       //SetRange(start_tmp, end_tmp);
       //EEPROM_UpdateChecksum();
       //EEPROM_Write();
     }
     if ((status.verbose == ON) || (parsed[parsed_tail].token == QUERY))
     {
       SDITask_Printf("\n Image Start: %04d", eeprom_base.eeprom.ImageStart);
       SDITask_Printf("\n Image End: %04d",  eeprom_base.eeprom.ImageEnd);
       SDITask_Printf("\n Range Start: %04d", eeprom_base.eeprom.RangeStart);
       SDITask_Printf("\n Range End: %04d",  eeprom_base.eeprom.RangeEnd);
     }
     else
     {
     }
}

void Raster_handler(void)
{
    Raster_cmd();
    //SDITask_Printf("\n Hello World \n");
}

#define PRINT_CHUNK     10

void read_handler(void)
{
    uint16_t i,j;
    uint16_t bin_value;

    /* Deal with Binning */
    if (eeprom_base.eeprom.ImagerBins > 1)
    {
        i = 0;
        while (i < (SPECTRUM_SIZE - eeprom_base.eeprom.ImagerBins))
        {
            bin_value = 0;
            for (j = 0; j < eeprom_base.eeprom.ImagerBins; j++)
            {
                bin_value += spectrum[i+j]/eeprom_base.eeprom.ImagerBins;
            }
            for (j = 0; j < eeprom_base.eeprom.ImagerBins; j++)
            {
                spectrum[i+j] = bin_value;
            }
            i += eeprom_base.eeprom.ImagerBins;
        }
    }
    /* Deal with filtering */
    if (eeprom_base.eeprom.GaussSigma != 0)
    {
        data_smooth(spectrum, SPECTRUM_SIZE);
    }
    if (status.Software == OFF)
    {
      for(i=0; i<= SPECTRUM_SIZE; i+=5)
      {
          SDITask_Printf("\n%u\n%u\n%u\n%u\n%u",spectrum[i],spectrum[i+1],spectrum[i+2],spectrum[i+3],spectrum[i+4]);
          usleep(10*1000);
      }
    }
    else
    {
        uint8_t bytes[64] = {0};
        for(int i=0; i < SPECTRUM_SIZE; i = i+32)
        {
            TakeSubSetPrepUART((uint16_t *)spectrum,bytes,i,i+32);
            if((i+32) > SPECTRUM_SIZE)
            {
                uint8_t length = 32 - ((i+32) - SPECTRUM_SIZE);
                SDITask_sendToUART(bytes,length*2);
            }
            else
            {
                SDITask_sendToUART(bytes,64);
            }
        }
    }
}

/*void referencing_handler(void)
{
    if (parsed[parsed_tail].token == SET)
      {
        status.Referencing = ON;
      }
      else if (parsed[parsed_tail].token == RESET)
      {
        status.Referencing = OFF;
      }
      else if (parsed[parsed_tail].token == SAVE)
      {
        if ((strcasecmp(parsed[parsed_tail].string[1], "ON")) == 0)
        {
          status.Referencing = ON;
          eeprom_base.eeprom.Referencing = ON;
          //EEPROM_Write();
          //SDITask_Printf("\n Success Referencing Set On");
        }
        else if ((strcasecmp(parsed[parsed_tail].string[1], "OFF")) == 0)
        {
          status.Referencing = OFF;
          eeprom_base.eeprom.Referencing = OFF;
         // EEPROM_Write();
          //SDITask_Printf("\n Success Referencing Set Off");
        }
      }

      if ((status.verbose == ON) || (parsed[parsed_tail].token == QUERY))
      {
        if (status.Referencing == ON)   SDITask_Printf("\nREFERENCING: ON");
        else if (status.Referencing == OFF) SDITask_Printf("\nREFERENCING: OFF");
        else SDITask_Printf("\nREFERENCING: INVALID STATE");
      }
      else
      {
      }
}*/

void reset_handler(void)
{
    //reset_cmd();
    HAL_SYSTEM_RESET();
}

void SerialNo_handler(void)
{
    if (parsed[parsed_tail].token == SET)
    {
        if(strlen(parsed[parsed_tail].string[1]) <= 15)
        {
            if (sscanf(parsed[parsed_tail].string[1], "%s", buffer) == EOF)
            {
            }
            strncpy(eeprom_base.eeprom.SerialNumber,buffer,SERIALNO_SIZE);
            SDITask_Printf("\n Success SerialNum set: %s",eeprom_base.eeprom.SerialNumber);
           // EEPROM_Write();
        }
        else
        {
            SDITask_Printf("\n SerialNum length must be 15 characters or less.", eeprom_base.eeprom.SerialNumber);
        }
    }
    else
    {
        SDITask_Printf("\n SerialNum: %s", eeprom_base.eeprom.SerialNumber);
    }
}

void Shutdown_handler(void)
{
    SDITask_Printf("\n System resetting...");
    //SysCtrlSystemReset();
    HAL_SYSTEM_RESET();
}

void integrate_handler(void)
{
    #  ifdef LAUNCHPAD
        unsigned int count = status.integration_time*status.Averages*100000*32;
        while(count--)
        {
            asm(" NOP");
        }
    # else
        IMX290_AcquireSpectrum(status.integration_time);
    # endif
}

void status_handler(void)
{
    //float tempf;
      switch (parsed[parsed_tail].token)
      {
      case SET:
        {
          //shutdown();
        }
        break;
      case TOK_ON:
        {
          if (status.safe == TRUE)
          {
              SDITask_Printf("\n SAFE");
          }
          else SDITask_Printf("\n UNSAFE");
        }
        break;
      case TOK_DEBUG:
        {
          if ((strcasecmp(parsed[parsed_tail].string[1], "ON")) == 0)
            eeprom_base.eeprom.FactoryMode = TRUE;
          else
            eeprom_base.eeprom.FactoryMode = FALSE;
         // EEPROM_Write();
        }
        break;
     /* case SELECT:
        {
          if ((strcasecmp(parsed[parsed_tail].string[1], "ON")) == 0)
          {
              eeprom_base.eeprom.Class1 = FALSE;
          }
          //EEPROM_Write();
        }
        break;*/
      /*case CLASS:
        {
          if ((strcasecmp(parsed[parsed_tail].string[1], "ON")) == 0)
          {
              eeprom_base.eeprom.Class1 = TRUE;
          }
          else eeprom_base.eeprom.Class1 = FALSE;
          //EEPROM_Write();
        }
        break;*/
      default:
        {
            SDITask_Printf("\n LD_TEMP = %4.1f", status.LD_Temp);
            /*SDITask_Printf("\n I_TEMP = %4.1f", status.Itemp);
            SDITask_Printf("\n I_TEMP_EXT = %4.1f", status.CCD_Ext_Temp);*/
          if (eeprom_base.eeprom.FactoryMode == TRUE)
          {
              SDITask_Printf("\n Factory Mode ON");
          }
          else
          {
              SDITask_Printf("\n Factory Mode OFF");
          }
          SDITask_Printf("\n PCB Revision  %3.1f", status.PCB_ID);

          /*if (eeprom_base.eeprom.Class1 == TRUE)
          {
              SDITask_Printf("\n Class 1 Mode");
          }*/
        }
        break;
      }  /* end switch on token */
}

/*void Stepper_handler(void)
{
    //Stepper_cmd();
}*/

/*void TBB_handler(void)
{
    //TBB_cmd();
}*/

void verbose_handler(void)
{
    if (parsed[parsed_tail].token == QUERY)
    {
        SDITask_Printf("\n Verbose: %d",status.verbose);
    }
    else if (parsed[parsed_tail].token == TOK_ON)
    {
        status.verbose = OFF;
        SDITask_Printf("\n Success Verbose: OFF");
    }
    else if (parsed[parsed_tail].token == TOK_OFF)
    {
        status.verbose = ON;
        SDITask_Printf("\n Success Verbose: ON");
    }
}

void software_handler(void)
{
    if (parsed[parsed_tail].token == QUERY)
    {
        SDITask_Printf("\n Software Mode: %u",status.Software);
    }
    else if (parsed[parsed_tail].token == TOK_ON)
    {
        status.Software = ON;
    }
    else if (parsed[parsed_tail].token == TOK_OFF)
    {
        status.Software = OFF;
    }
}

void id_handler(void)
{
    if (parsed[parsed_tail].token == QUERY)
      {
        SDITask_Printf("\n Model: %s", ID_STR);
        SDITask_Printf("\n Serial: %s",eeprom_base.eeprom.SerialNumber);
        SDITask_Printf("\n Version: %s", SHORTVER);
      }
      /*else if (parsed[parsed_tail].token == READ)
      {
          SDITask_Printf("\nPIN: %s", eeprom.PinCode);
      }
      else if (parsed[parsed_tail].token == SET)
      {
        eeprom.PinCode[0] = parsed[parsed_tail].string[1][0];
        eeprom.PinCode[1] = parsed[parsed_tail].string[1][1];
        eeprom.PinCode[2] = parsed[parsed_tail].string[1][2];
        eeprom.PinCode[3] = parsed[parsed_tail].string[1][3];
        eeprom.PinCode[4] = 0;
        EEPROM_UpdateChecksum();
        SDITask_Printf("\nNew PIN: %s", eeprom.PinCode);
      }*/
}

void null_handler(void)
{

}
