#include <stdio.h>

#define SCPI_ARGS_N_C 2
#define SCPI_ARGS_N_Q 0
#include "app/scpi/scpi_handler.h"
#include "app/control_table/control_table.h"
#include "drivers/switchboard_control/switchboard_control.h"

// -----
// @argTypesCommand
// Declare argument parser entities for command form
// Supported arguments: 1=NUMERIC
DECLARE_SCPI_ARGS_C( eScpiArg_Numeric, eScpiArg_Numeric );
// -----
// @argTypesQuery
// Declare argument parser entities for query form
// Supported arguments: 0
DECLARE_SCPI_ARGS_Q();

DECLARE_ARGUMENT_NUMERIC_VALUES_I32(AllowedValues_SW_Number, 1, 10);
DECLARE_ARGUMENT_NUMERIC_VALUES_I32(AllowedValues_SW_State, 0, 3);

const uint8_t fsqvbl_CommandHandlerSwCtrl     = 1;  // CTRL:SW
const uint8_t fsqvbl_CommandHandlerMeasStart  = 2;  // MEAS:START
const uint8_t fsqvbl_CommandHandlerMeasStop   = 3;  // MEAS:STOP
const uint8_t fsqvbl_CommandHandlerMeasCont   = 4;  // MEAS:CONTinue
const uint8_t fsqvbl_CommandHandlerMeasMode   = 5;  // MEAS:MODE

static uint16_t sw_cmd_reg = 0x0000;

#include "app/scpi/commandHandlers/power_switch.h"
#include "app/nfm/nfm_base.h"

// Refer to:
// [1] SCPI Specification, revision 1999.0
//     "Standard Commands for Programmable Instruments (SCPI), VERSION 1999.0, May 1999"
// [2] Gpib Programming Tutorial, (http://g2pc1.bu.edu/~qzpeng/gpib/manual/GpibProgTut.pdf)
//     Electronics Group (http://www.few.vu.nl/~elec), 11 January 2000 Electronics Group
// [3] IEEE 488.2 Standard, revision IEEE Std 488.2-1987 (1992)
//     "IEEE Standard Codes, Formats, Protocols, and Common Commands for Use With IEEE Std 488.1-1987, IEEE

// =================================================================================
// @fsqvbl_CommandHandlerMEASnSWITCH_group
// State's virtual table

static void fsqe_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, tFSeqCtx_t ctx );
static void fsql_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, tFSeqCtx_t ctx );
static const struct fFSeqEntry_t * fsqf_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, tFSeqCtx_t ctx, const struct fFSeqEntry_t * * pDeferredNext );

const fFSeqVTable_t fsqvbl_CommandHandlerCtrl_group = 
{
     .f = fsqf_CommandHandlerCtrl_Switch_Group,
     .enter = fsqe_CommandHandlerCtrl_Switch_Group,  
     .leave = fsql_CommandHandlerCtrl_Switch_Group
};

static void fsqe_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, tFSeqCtx_t ctx )
{
    sProcessProgramDataCommonContext_t * common_ctx = ctx;
    
    common_ctx->MeasAndSwitch.idx = 0;
    
    SCPI_PARSE_ARGUMENTS( common_ctx ); (void)common_ctx->argsParserStatus; // status is modified
}

static void fsql_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, tFSeqCtx_t ctx )
{
}

// Set bit into possition func
uint16_t set_bit(uint16_t cmd_register, uint16_t SW_Number, uint16_t SW_State) {
   
  unsigned char bit_position=0;
  
  switch (SW_Number)
  {
  case 1: {bit_position = 1;} break;
  case 2: {bit_position = 9;} break;
  case 3: {bit_position = 2;} break;
  case 4: {bit_position = 10;} break;
  case 5: {bit_position = 3;} break;
  case 6: {bit_position = 4;} break;
  case 7: {bit_position = 12;} break;
  case 8: {bit_position = 13;} break;
  case 9: {bit_position = 14;} break;
  case 10: {bit_position = 15;} break;
  }

  if (SW_Number == 4)
  {      // clear control bits positions
        cmd_register &= ~(3 << bit_position);
        // set control bits positions
        cmd_register  |= (SW_State << bit_position);
  }
  else
  {      // reset bit in possition
        cmd_register &= ~(1 << bit_position);
        cmd_register  |= ((SW_State & 1) << bit_position);
  } 
  
  return cmd_register;
}


static const struct fFSeqEntry_t * fsqf_CommandHandlerCtrl_Switch_Group( const struct fFSeqEntry_t * this, 
                                                                         tFSeqCtx_t ctx, 
                                                                          const struct fFSeqEntry_t * * pDeferredNext )
{
    const fFSeqEntry_t * nextstate = NULL;
    
    sProcessProgramDataCommonContext_t * common_ctx = ctx;
    sScpiParserContext_t * global_ctx = common_ctx->global_ctx;

    switch( common_ctx->event )
    {
        case eProgramData_Event_Write:
        {
            if( eScpiStatus_success != common_ctx->argsParserStatus ) // check argument parser status
            {
               common_ctx->status = eProgramDataArgumentSyntax;  // parameter syntax error, caller should generate error message
            }
            else if( ! common_ctx->isQuery )
            {
               common_ctx->status = eProgramDataIllegalArgument;  // forward set, illegal parameter value, caller should generate error message
               if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerSwCtrl )
               {
                    // Parse first argument
                    const sNumericEntry_t * ne = SCPI_PROCESS_ARGUMENT_NUMERIC(common_ctx, &AllowedValues_SW_Number, 0);
                    if( ScpiNumericSuccess != ne->error ) break;
                    uint16_t swNumber = ne->Value.demicalInteger;
                    
                    // Parse second argument
                    ne = SCPI_PROCESS_ARGUMENT_NUMERIC(common_ctx, &AllowedValues_SW_State, 1);
                    if( ScpiNumericSuccess != ne->error ) break;
                    uint16_t swState = ne->Value.demicalInteger;
                    
                    sw_cmd_reg = set_bit(sw_cmd_reg, swNumber, swState);
                        
                    HAL_GPIO_WritePin( GPIOA, GPIO_PIN_8, GPIO_PIN_SET);  // Turn OFF active Reset signal on external shift REG that was set during powerup
                    
                    SBHandle.ClrNSS();
                    SBHandle.Transmit(((uint8_t*)&(sw_cmd_reg)), sizeof(uint8_t));
                    SBHandle.SetNSS();
                   
                    
                    common_ctx->status = eProgramDataDone;
               }
            }
            else
            {
                 common_ctx->status = eProgramDataNeedRead;  // request processed, wait for reading...
                 (void)nextstate; // stay in this state
            }
        }
        break;

        case eProgramData_Event_Read:
        {
             // @idx - current position of the source data to be outputed
             if( common_ctx->MeasAndSwitch.idx == 0 ) // first reading
             {
                 size_t length = 0;                 
                 if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerSwCtrl )
                 {
                     length = _snprintf( common_ctx->tempBuffer, sizeof(common_ctx->tempBuffer), "%d", sw_cmd_reg);
                 }
                 else if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerMeasStart )
                 {
                     ControlHandle.Start_Meas();
                     length = _snprintf( common_ctx->tempBuffer, sizeof(common_ctx->tempBuffer), "%s", "OK\n");
                 }
                 else if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerMeasStop )
                 {
                     ControlHandle.Stop_Meas();
                     length = _snprintf( common_ctx->tempBuffer, sizeof(common_ctx->tempBuffer), "%s", "OK\n");
                 }
                 else if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerMeasCont )
                 {
                     ControlHandle.Continue_Meas();
                     length = _snprintf( common_ctx->tempBuffer, sizeof(common_ctx->tempBuffer), "%s", "OK\n");
                 }
                 else if( common_ctx->handler_ctx == &fsqvbl_CommandHandlerMeasMode )
                 {
                     uint8_t res = ControlHandle.Mode_Meas();
                     if(res != ERROR_CODE)
                     {
                         length = _snprintf( common_ctx->tempBuffer, sizeof(common_ctx->tempBuffer), "%s", res ? "TABLE\n" : "MANUAL\n");
                     }
                 }
                 

				 
                 if( length > 0 )
                 {
                     // place null-terminator in the end of line
                     common_ctx->tempBuffer[length] = '\0';
                 }
                 else
                 {
                     
                     fsq_RaiseError( SCPI_ERROR_INTERNAL_DEVICE,
                                     SCPI_ERROR_INTERNAL_DEVICE_MSG,
                                     global_ctx->sParser.xHandlerToken.shead,
                                     global_ctx->sParser.xHandlerToken.stail );

                     common_ctx->status = eProgramData_SpecificError; // specific error already generated
                     break;
                 }
             }

             // Since @done flag is set, this dispatcher shall not be called anymore.
             // Since this handler is implemented as a single-state automat, there no
             // ... other states to go to:
             (void)nextstate;

             // modify current postion index:
             SCPI_RESPONSE_HELPER( common_ctx, common_ctx->MeasAndSwitch.idx );
        }
        break;
    }

    return nextstate;
}
