#include <scpiparser.h>
#include <Arduino.h>

#define NUMBEROFCHANNELS 32


struct scpi_parser_context ctx;


scpi_error_t identify(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t reset(struct scpi_parser_context* context, struct scpi_token* command);

scpi_error_t get_digital_1(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_2(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_3(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_4(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_5(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_6(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_7(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_8(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_9(struct scpi_parser_context* context, struct scpi_token* command);

scpi_error_t get_digital_10(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_11(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_12(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_13(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_14(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_15(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_16(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_17(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_18(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_19(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_20(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_21(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_22(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_23(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_24(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_25(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_26(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_27(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_28(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_29(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_30(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_31(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t get_digital_32(struct scpi_parser_context* context, struct scpi_token* command);



scpi_error_t set_digital_1(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_2(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_3(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_4(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_5(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_6(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_7(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_8(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_9(struct scpi_parser_context* context, struct scpi_token* command);

scpi_error_t set_digital_10(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_11(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_12(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_13(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_14(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_15(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_16(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_17(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_18(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_19(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_20(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_21(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_22(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_23(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_24(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_25(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_26(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_27(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_28(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_29(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_30(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_31(struct scpi_parser_context* context, struct scpi_token* command);
scpi_error_t set_digital_32(struct scpi_parser_context* context, struct scpi_token* command);






bool bDigital [NUMBEROFCHANNELS] = {};
int  iPins [NUMBEROFCHANNELS] = {22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53};


void setup() {
  struct scpi_command* digital;
  int i = 0;
  Serial.begin(9600);
  
  for (i = 0; i < NUMBEROFCHANNELS; i++) {
    pinMode(iPins[i], OUTPUT);
    digitalWrite(iPins[i], HIGH);    
  }




  /* First, initialise the parser. */
  scpi_init(&ctx);


  /*
   * After initialising the parser, we set up the command tree.  Ours is
   *
   *  *IDN?         -> identify
   *  *RST          -> reset
   * 
   *  :DIGI
   *    :SWIT1    -> channel 1
   *    :SWIT2    -> channel 2
   *    :SWIT3    -> channel 3
   *    :SWIT4    -> channel 4
   *    :SWIT5    -> channel 5
   *    :SWIT6    -> channel 6
   *    :SWIT7    -> channel 7
   *    :SWIT8    -> channel 8
   *    :SWIT9    -> channel 9
   *    :SWIT10   -> channel 10
   *    :SWIT11   -> channel 11
   *    :SWIT12   -> channel 12
   *    :SWIT13   -> channel 13
   *    :SWIT14   -> channel 14
   *    :SWIT15   -> channel 15
   *    :SWIT16   -> channel 16
   *    :SWIT17   -> channel 17
   *    :SWIT18   -> channel 18
   *    :SWIT19   -> channel 19
   *    :SWIT20   -> channel 20
   *    :SWIT21   -> channel 21
   *    :SWIT22   -> channel 22
   *    :SWIT23   -> channel 23
   *    :SWIT24   -> channel 24
   *    :SWIT25   -> channel 25
   *    :SWIT26   -> channel 26
   *    :SWIT27   -> channel 27
   *    :SWIT28   -> channel 28
   *    :SWIT29   -> channel 29
   *    :SWIT30   -> channel 30
   *    :SWIT31   -> channel 31
   *    :SWIT32   -> channel 32
   * 
   *    :CELL1    -> channel 1/2
   *    :CELL2    -> channel 3/4
   *    :CELL3    -> channel 5/6
   *    :CELL4    -> channel 7/8
   *    :CELL5    -> channel 9/10
   *    :CELL6    -> channel 11/12
   *    :CELL7    -> channel 13/14
   *    :CELL8    -> channel 15/16
   *    :CELL9    -> channel 17/18
   *    :CELL10   -> channel 19/20
   *    :CELL11   -> channel 21/22
   *    :CELL12   -> channel 23/24
   *    :CELL13   -> channel 25/26
   * 
   *    :POSMESS    -> channel 27/28
   *    :NEGMESS    -> channel 29/30 
   */
  scpi_register_command(ctx.command_tree, SCPI_CL_SAMELEVEL, "*IDN?", 5, "*IDN?", 5, identify);
  scpi_register_command(ctx.command_tree, SCPI_CL_SAMELEVEL, "*RST", 4, "*RST", 4, reset);
  scpi_register_command(ctx.command_tree, SCPI_CL_SAMELEVEL, "*CLS", 4, "*CLS", 4, reset);


  digital = scpi_register_command(ctx.command_tree, SCPI_CL_CHILD, "DIGITAL", 7, "DIGI", 4, NULL);


  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH1?", 8, "SWIT1?", 6, get_digital_1);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH2?", 8, "SWIT2?", 6, get_digital_2);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH3?", 8, "SWIT3?", 6, get_digital_3);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH4?", 8, "SWIT4?", 6, get_digital_4);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH5?", 8, "SWIT5?", 6, get_digital_5);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH6?", 8, "SWIT6?", 6, get_digital_6);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH7?", 8, "SWIT7?", 6, get_digital_7);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH8?", 8, "SWIT8?", 6, get_digital_8);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH9?", 8, "SWIT9?", 6, get_digital_9);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH10?", 8, "SWIT10?", 6, get_digital_10);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH11?", 8, "SWIT11?", 6, get_digital_11);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH12?", 8, "SWIT12?", 6, get_digital_12);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH13?", 8, "SWIT13?", 6, get_digital_13);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH14?", 8, "SWIT14?", 6, get_digital_14);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH15?", 8, "SWIT15?", 6, get_digital_15);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH16?", 8, "SWIT16?", 6, get_digital_16);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH17?", 8, "SWIT17?", 6, get_digital_17);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH18?", 8, "SWIT18?", 6, get_digital_18);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH19?", 8, "SWIT19?", 6, get_digital_19);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH20?", 8, "SWIT20?", 6, get_digital_20);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH21?", 8, "SWIT21?", 6, get_digital_21);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH22?", 8, "SWIT22?", 6, get_digital_22);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH23?", 8, "SWIT23?", 6, get_digital_23);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH24?", 8, "SWIT24?", 6, get_digital_24);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH25?", 8, "SWIT25?", 6, get_digital_25);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH26?", 8, "SWIT26?", 6, get_digital_26);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH27?", 8, "SWIT27?", 6, get_digital_27);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH28?", 8, "SWIT28?", 6, get_digital_28);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH29?", 8, "SWIT29?", 6, get_digital_29);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH30?", 8, "SWIT30?", 6, get_digital_30);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH31?", 8, "SWIT31?", 6, get_digital_31);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH32?", 8, "SWIT32?", 6, get_digital_32);

  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH1", 7, "SWIT1", 5, set_digital_1);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH2", 7, "SWIT2", 5, set_digital_2);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH3", 7, "SWIT3", 5, set_digital_3);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH4", 7, "SWIT4", 5, set_digital_4);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH5", 7, "SWIT5", 5, set_digital_5);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH6", 7, "SWIT6", 5, set_digital_6);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH7", 7, "SWIT7", 5, set_digital_7);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH8", 7, "SWIT8", 5, set_digital_8);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH9", 7, "SWIT9", 5, set_digital_9);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH10", 7, "SWIT10", 5, set_digital_10);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH11", 7, "SWIT11", 5, set_digital_11);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH12", 7, "SWIT12", 5, set_digital_12);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH13", 7, "SWIT13", 5, set_digital_13);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH14", 7, "SWIT14", 5, set_digital_14);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH15", 7, "SWIT15", 5, set_digital_15);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH16", 7, "SWIT16", 5, set_digital_16);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH17", 7, "SWIT17", 5, set_digital_17);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH18", 7, "SWIT18", 5, set_digital_18);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH19", 7, "SWIT19", 5, set_digital_19);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH20", 7, "SWIT20", 5, set_digital_20);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH21", 7, "SWIT21", 5, set_digital_21);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH22", 7, "SWIT22", 5, set_digital_22);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH23", 7, "SWIT23", 5, set_digital_23);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH24", 7, "SWIT24", 5, set_digital_24);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH25", 7, "SWIT25", 5, set_digital_25);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH26", 7, "SWIT26", 5, set_digital_26);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH27", 7, "SWIT27", 5, set_digital_27);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH28", 7, "SWIT28", 5, set_digital_28);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH29", 7, "SWIT29", 5, set_digital_29);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH30", 7, "SWIT30", 5, set_digital_30);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH31", 7, "SWIT31", 5, set_digital_31);
  scpi_register_command(digital, SCPI_CL_CHILD, "SWITCH32", 7, "SWIT32", 5, set_digital_32);





}


void loop() {
  char line_buffer[256];
  unsigned char read_length;


  while(1)   {
    /* Read in a line and execute it. */
    read_length = Serial.readBytesUntil('\n', line_buffer, 256);  
    if(read_length >0)  
      if(line_buffer[read_length-1]=='\r')  // thank you jon clift
        read_length = read_length - 1;    if(read_length > 0)     {
      scpi_execute_command(&ctx, line_buffer, read_length);
    }
  }
}




 


/*
 * Respond to *IDN?
 */
scpi_error_t identify(struct scpi_parser_context* context, struct scpi_token* command) {
  scpi_free_tokens(command);


  Serial.println("Relaycontrolunit 1,SCPI Switch,v0.01,");
  return SCPI_SUCCESS;
}


/*
 * Respond to *RST and *CLS
 */
scpi_error_t reset(struct scpi_parser_context* context, struct scpi_token* command) {
  scpi_free_tokens(command);
  int i = 0;


  for (i = 0; i < NUMBEROFCHANNELS; i++) {
    digitalWrite(iPins[i], HIGH);
    bDigital[i] = 0;    
  }
  return SCPI_SUCCESS;
}




scpi_error_t get_digital(int channel, struct scpi_parser_context* context, struct scpi_token* command) {
  if (bDigital[channel] ) {
    Serial.println("ON");
  } else {
    Serial.println("OFF");
   
  }
  scpi_free_tokens(command);
  return SCPI_SUCCESS;  
}




/**
 * get digital 1 status
 */
scpi_error_t get_digital_1(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(0, context, command);
}
/**
 * get digital 2 status
 */
scpi_error_t get_digital_2(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(1, context, command);
}
/**
 * get digital 3 status
 */
scpi_error_t get_digital_3(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(2, context, command);
}
/**
 * get digital 4 status
 */
scpi_error_t get_digital_4(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(3, context, command);
}
/**
 * get digital 5 status
 */
scpi_error_t get_digital_5(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(4, context, command);
}
/**
 * get digital 6 status
 */
scpi_error_t get_digital_6(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(5, context, command);
}
/**
 * get digital 7 status
 */
scpi_error_t get_digital_7(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(6, context, command);
}
/**
 * get digital 8 status
 */
scpi_error_t get_digital_8(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(7, context, command);
}
/**
 * get digital 9 status
 */
scpi_error_t get_digital_9(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(8, context, command);
}
/**
 * get digital 10 status
 */
scpi_error_t get_digital_10(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(9, context, command);
}
/**
 * get digital 11 status
 */
scpi_error_t get_digital_11(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(10, context, command);
}
/**
 * get digital 12 status
 */
scpi_error_t get_digital_12(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(11, context, command);
}
/**
 * get digital 13 status
 */
scpi_error_t get_digital_13(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(12, context, command);
}
/**
 * get digital 14 status
 */
scpi_error_t get_digital_14(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(13, context, command);
}
/**
 * get digital 15 status
 */
scpi_error_t get_digital_15(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(14, context, command);
}
/**
 * get digital 16 status
 */
scpi_error_t get_digital_16(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(15, context, command);
}
/**
 * get digital 17 status
 */
scpi_error_t get_digital_17(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(16, context, command);
}
/**
 * get digital 18 status
 */
scpi_error_t get_digital_18(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(17, context, command);
}
/**
 * get digital 19 status
 */
scpi_error_t get_digital_19(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(18, context, command);
}
/**
 * get digital 20 status
 */
scpi_error_t get_digital_20(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(19, context, command);
}
/**
 * get digital 21 status
 */
scpi_error_t get_digital_21(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(20, context, command);
}
/**
 * get digital 22 status
 */
scpi_error_t get_digital_22(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(21, context, command);
}
/**
 * get digital 23 status
 */
scpi_error_t get_digital_23(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(22, context, command);
}
/**
 * get digital 24 status
 */
scpi_error_t get_digital_24(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(23, context, command);
}
/**
 * get digital 25 status
 */
scpi_error_t get_digital_25(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(24, context, command);
}
/**
 * get digital 26 status
 */
scpi_error_t get_digital_26(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(25, context, command);
}
/**
 * get digital 27 status
 */
scpi_error_t get_digital_27(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(26, context, command);
}
/**
 * get digital 28 status
 */
scpi_error_t get_digital_28(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(27, context, command);
}
/**
 * get digital 29 status
 */
scpi_error_t get_digital_29(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(28, context, command);
}
/**
 * get digital 30 status
 */
scpi_error_t get_digital_30(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(29, context, command);
}
/**
 * get digital 31 status
 */
scpi_error_t get_digital_31(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(30, context, command);
}
/**
 * get digital 32 status
 */
scpi_error_t get_digital_32(struct scpi_parser_context* context, struct scpi_token* command) {
  return get_digital(31, context, command);
}


scpi_error_t set_digital(int channel, struct scpi_parser_context* context, struct scpi_token* command) {
  struct scpi_token* args;
  bool bSuccess = false;
  bool bSwitch = false;


  args = command;
  while(args != NULL && args->type == 0) {
    args = args->next;
  }


  if (args->length == 1) {
    if (args->value[0] == '0') {
      bSuccess = true;
    }
    if (args->value[0] == '1') {
      bSuccess = true;
      bSwitch = true;
    }
  }


  if (args->length == 2) {
    if ((args->value[0] == 'O') || (args->value[0] == 'o')) {
      if ((args->value[1] == 'N') || (args->value[1] == 'n')) {
        bSuccess = true;
        bSwitch = true;
      }    
    }
  }


  if (args->length == 3) {
    if ((args->value[0] == 'O') || (args->value[0] == 'o')) {
      if ((args->value[1] == 'F') || (args->value[1] == 'f')) {
        if ((args->value[2] == 'F') || (args->value[2] == 'f')) {
        bSuccess = true;
        }
      }
    }
  }


  scpi_free_tokens(command);


//  Serial.print("Success: ");
//  Serial.print(bSuccess ? "y" : "n");
//  Serial.print(" channel: ");
//  Serial.print(channel);
//  Serial.print(" pin ");
//  Serial.print(iPins[channel]);
//  Serial.print(" switch ");
//  Serial.print(bSwitch ? "on" : "off");
//  Serial.print("\n");


  if (bSuccess) {    
    digitalWrite(iPins[channel], bSwitch? LOW : HIGH);
    bDigital[channel] = bSwitch;
  }
  
  return bSuccess ? SCPI_SUCCESS : SCPI_COMMAND_NOT_FOUND;
 
}


scpi_error_t set_digital_1(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(0, context, command);
}

scpi_error_t set_digital_2(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(1, context, command);
}

scpi_error_t set_digital_3(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(2, context, command);
}

scpi_error_t set_digital_4(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(3, context, command);
}

scpi_error_t set_digital_5(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(4, context, command);
}

scpi_error_t set_digital_6(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(5, context, command);
}

scpi_error_t set_digital_7(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(6, context, command);
}

scpi_error_t set_digital_8(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(7, context, command);
}

scpi_error_t set_digital_9(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(8, context, command);
}

scpi_error_t set_digital_10(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(9, context, command);
}

scpi_error_t set_digital_11(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(10, context, command);
}

scpi_error_t set_digital_12(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(11, context, command);
}

scpi_error_t set_digital_13(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(12, context, command);
}

scpi_error_t set_digital_14(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(13, context, command);
}

scpi_error_t set_digital_15(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(14, context, command);
}

scpi_error_t set_digital_16(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(15, context, command);
}

scpi_error_t set_digital_17(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(16, context, command);
}

scpi_error_t set_digital_18(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(17, context, command);
}

scpi_error_t set_digital_19(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(18, context, command);
}

scpi_error_t set_digital_20(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(19, context, command);
}

scpi_error_t set_digital_21(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(20, context, command);
}

scpi_error_t set_digital_22(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(21, context, command);
}

scpi_error_t set_digital_23(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(22, context, command);
}

scpi_error_t set_digital_24(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(23, context, command);
}

scpi_error_t set_digital_25(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(24, context, command);
}

scpi_error_t set_digital_26(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(25, context, command);
}

scpi_error_t set_digital_27(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(26, context, command);
}

scpi_error_t set_digital_28(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(27, context, command);
}

scpi_error_t set_digital_29(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(28, context, command);
}

scpi_error_t set_digital_30(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(29, context, command);
}

scpi_error_t set_digital_31(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(30, context, command);
}

scpi_error_t set_digital_32(struct scpi_parser_context* context, struct scpi_token* command) {
  return set_digital(31, context, command);
}