/*
  control sercalo fiber-optic switch sw 2x2

  This sketch uses the Serial.read() function in order to control a MEMS-Switch
  A button triggers also the switching.
  
  Circuit:
  - digital pin 2 for switch
  - digital pin 3 & 4 for LEDs
  - digital pin 12 for button
  - GND
  - +5V
  
  Command set:
  :ROUT:CLOS <D>,<Input>,<Output>
  :ROUT:CLOS? <D>,<Input> -> <Output>
  :LOCK <STATE> (state 0:unlock; 1:lock)
  :LOCK? -> <STATE>
  :LOCK:RELE
  :SYST:ERR? -> <error number>, <error message>
  *STB -> <status byte>
  *IDN? -> <Manufacturer>,<Model>,<ID>,<FW>
  :CONF? -> <Device>,<Type>,<Maxin>,<Maxout>,<Latching>,<max Trim level>,<Opm>,<Fiber Mode >,
            <Fiber Core Size>,<Connector Type>
  :INFO? -> <Serial Number>,<Part Number>,<Card Firmware Rev>,<OSW Module Firmware Rev>,
            <OPM Module firmware Rev>,<HW Rev>,<Assembly Date>,<Description>
  :DEV:INFO? <D> -> <switch serial number>,<switch model number>

   created 8 August 2022
   by Gregor Popp
   modified 14 November 2022
   by Gregor Popp

*/

#include <ctype.h>

// parameters of button
const int  buttonPin = 12;
int buttonState = 0;         // current state of the button
int lastButtonState = 0;     // previous state of the button

// parameter of indicator leds
const int ledPin1 = 3;
const int ledPin2 = 4;
const int ledPinLock = 18; // 21

// parameters of optical switches
const int firstPins[] = {2, 3}; // ends with: last pin of device +1
const int nPins = firstPins[1] - firstPins[0];
const int nSwitches = (sizeof (firstPins) / sizeof (*firstPins)) -1;
const int oswPattern[] = {0, 1};
const int len = (sizeof (oswPattern) / sizeof (*oswPattern));
const int minimumValue = 1;
const int maximumValue = 2;
const String IDN = "SERCALO,SN2x2,103597,1.0";
// Manufacturer,Model,ID,FW
const String CONFIG = "1 OSW 2 2 NO 0 NO 0 9.0 FC/APC";
const String INFO = "103597,SN2x2-9N-FC/APC,1.0.0,n/a,n/a,1.0,20220929,USB OPTICAL SWITCH";
// <Serial Number>,<Part Number>,<Card Firmware Rev>,<OSW Module Firmware Rev>,
// <OPM Module firmware Rev>,<HW Rev>,<Assembly Date>,<Description>
char *DEVINFO[] = {"2022-39-103597,SN2x2"};
// <switch serial number>,<switch model number>
const int errN[] = {0, -102, -224, -227, -222, -227, -240};
char *errText[] = {"\"no error\"", "\"Syntax error\"", "\"Illegal parameter value\"",
                   "\"Invalid device address\"", "\"Data out of range\"",
                   "\"Invalid address or no card inserted\"",
                   "\"Hardware error\""
                  };

const byte numChars = 32;
int errornumber = 0;
byte nTries = 3; // Anzahl Versuche falls digitale Ausgänge abweichend zu Schalterwunsch

struct parsedDataS {
  char command[numChars] = {0};
  int device = 0;
  int inputChannel = 0;
  int outputChannel = 0;
};

struct recvDataS {
  char receivedChars[numChars] = {0};
  boolean newData = false;
};

bool lock = false;

//============

void setup() {
  Serial.begin(9600);
  Serial.println();
  for (int j = 0; j < nSwitches; j++) {
    for (int i = 0; i < nPins; i++) {
      pinMode(i + firstPins[j], OUTPUT);
    }
  }
  // button
  pinMode(buttonPin, INPUT_PULLUP);

  // Leds
  pinMode(ledPin1, OUTPUT);
  pinMode(ledPin2, OUTPUT);
  pinMode(ledPinLock, OUTPUT);

  setdigitalout(1,1); // Initialisierung Schaltposition nach Einschalten
}

//============

void loop() {
  char receivedChars[numChars];
  recvDataS rB;
  parsedDataS rP;
  char tempChars[numChars]; // temporary array for use when parsing

  rB = recvWithStartEndMarkers();
  if (rB.newData == true) {
    strcpy(tempChars, rB.receivedChars);
    // this temporary copy is necessary to protect the original data
    //   because strtok() used in parseData() replaces the commas with \0
    rP = parseData(tempChars);
    selectCommand(rP);
  }
  else {
    if (!lock) { // Software Lock 
      buttonState = digitalRead(buttonPin);
      if (lastButtonState == HIGH && buttonState == LOW) {
        //digitalWrite(ledPin2, !digitalRead(ledPin2)); // #stattdessen Ch+1
        setdigitalout(1, 3-getdigitalout(1));
      }
      lastButtonState = buttonState;
    }
    delay(200);
  }
}

recvDataS recvWithStartEndMarkers() {
  boolean newDataRecv = false; // newDataRecv = newData
  char receivedChars[numChars];
  recvDataS rbuf;

  static boolean recvInProgress = false;
  static byte ndx = 0;
  char startMarkerCCQH = '*';
  //Common Command and Query Headers
  char startMarkerIQH = ':';
  // Instrument-Control Headers
  char endMarker = '\n';
  char rc;

  while (Serial.available() > 0 && newDataRecv == false) {
    rc = Serial.read();
    rc = toupper(rc);

    if (recvInProgress == true) {
      if (rc != endMarker) {
        receivedChars[ndx] = rc;
        ndx++;
        if (ndx >= numChars) {
          ndx = numChars - 1;
        }
      }
      else {
        receivedChars[ndx] = '\0'; // terminate the string
        recvInProgress = false;
        ndx = 0;
        newDataRecv = true;
      }
    }
    else if (rc == startMarkerCCQH || rc == startMarkerIQH) {
      recvInProgress = true;
    }
  }
  strcpy(rbuf.receivedChars, receivedChars);
  rbuf.newData = newDataRecv;
  return rbuf;
}

//============

parsedDataS parseData(char tempChars[numChars]) {      // split the data into its parts
  parsedDataS pbuf;
  char * strtokIndx; // this is used by strtok() as an index

  strtokIndx = strtok(tempChars, " ");     // get the first part - the string
  strcpy(pbuf.command, strtokIndx); // copy it to command

  strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
  pbuf.device = atoi(strtokIndx);     // convert this part to an integer

  strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
  pbuf.inputChannel = atoi(strtokIndx);     // convert this part to an integer

  strtokIndx = strtok(NULL, ","); // this continues where the previous call left off
  pbuf.outputChannel = atoi(strtokIndx);     // convert this part to an integer

  return pbuf;
}

//============

void selectCommand(parsedDataS pBuf) {
  if (strcmp(pBuf.command, "IDN?") == 0) {
    // *idn?
    Serial.println(IDN);
  }
  else if (strcmp(pBuf.command, "ROUT:CLOS") == 0) {
    // :ROUTe:CLOSe <D>,<Input>,<Output>
    // :ROUT:CLOS 1,1,20
    routeClose(pBuf);
  }
  else if (strcmp(pBuf.command, "ROUT:CLOS?") == 0) {
    // :ROUTe:CLOSe? <D>,<Input>
    // <Output>
    queryRouteClose(pBuf);
  }
  else if (strcmp(pBuf.command, "LOCK:RELE") == 0) {
  //:LOCK:RELE
    lock = 0; 
    digitalWrite(ledPinLock, LOW);
  }
  else if (strcmp(pBuf.command, "LOCK") == 0) {
    // :LOCK <STATE>[,<OWNER_NAME>,<IP_ID>]
    if ( inRange(pBuf.device, 0, 1) ) {
      // check lock status in range
      lock = pBuf.device;
      if (lock==1) {
        digitalWrite(ledPinLock, HIGH);
      }
      else {
        digitalWrite(ledPinLock, LOW);
        }
    }
  }
  else if (strcmp(pBuf.command, "LOCK?") == 0) {
    // :LOCK?
    Serial.println(lock);
  }
  else if (strcmp(pBuf.command, "CONF?") == 0) {
    // :CONF?
    // :CONFig?
    Serial.println(CONFIG);
  }
  else if (strcmp(pBuf.command, "INFO?") == 0) {
    // :INFOrmation?
    // :INFO?
    Serial.println(INFO);
  }
  else if (strcmp(pBuf.command, "DEV:INFO?") == 0) {
    // :DEVice:INFOrmation? <D>
    if ( inRange(pBuf.device, 1, nSwitches) ) {
      // check <D> in range
      Serial.println(DEVINFO[pBuf.device - 1]);
    }
    else {
      errornumber = 2;
    }
  }
  else if (strcmp(pBuf.command, "SYST:ERR?") == 0) {
    querySysErr();
  }
  else if (strcmp(pBuf.command, "STB?") == 0) {
    // *STB?
    Serial.println("*STB?");
    Serial.println((errornumber > 0) << 5);
  }
  else {
    errornumber = 1;
  }
}

//============

bool inRange(int val, int minimum, int maximum)
{
  return ((minimum <= val) && (val <= maximum));
}

//============

void routeClose(parsedDataS pBuf) {
  if ( inRange(pBuf.device, 1, nSwitches) )  {
    // Erweiterung device == 0 -> alle Schalter schalten
    if ( inRange(pBuf.inputChannel, 1, 1) ) {
      // vorerst haben alle Schalter nur einen inputChannel
      if ( inRange(pBuf.outputChannel, minimumValue, maximumValue) ) {
        // check D, input, output in range
        setdigitalout(pBuf.device, pBuf.outputChannel);
      }
      else {
        errornumber = 2;
      }
    }
    else {
      errornumber = 2;
    }
  }
  else {
    errornumber = 3;
  }
}

//============

void queryRouteClose(parsedDataS pBuf) {
  // check D,Input in range
  // check Output plausible
  int readOutputChannel = -1;
  if ( inRange(pBuf.device, 1, nSwitches) )  {
    if ( inRange(pBuf.inputChannel, 1, 1) ) {
      readOutputChannel = getdigitalout(pBuf.device);
      // Plausibilitätsprüfung
      if (!inRange(readOutputChannel, minimumValue, maximumValue) ) {
        readOutputChannel = -1;
        errornumber = 4;
      }
    }
    else {
      errornumber = 2;
    }
  }
  else {
    errornumber = 3;
  }
  Serial.println(readOutputChannel);
}
//============

void querySysErr() {
  errornumber = abs(errornumber); // Vorbeugung
  Serial.print(errN[errornumber]);
  Serial.print(", ");
  Serial.println(errText[errornumber]);
  errornumber = 0;
}

//============

void setdigitalout(int device, int switch_pos) {
  int g;
  int a = oswPattern[switch_pos - 1];

  for (int j = 0; j < nTries; j++) {
    for (int i = 0; i < nPins; i++) {
      digitalWrite(i + firstPins[device - 1], (a & 1 << i));
    }
    g = getdigitalout(device);
    if (switch_pos == g) {
      break;
    }
  }
  if (switch_pos != g) {
    errornumber = 6; // Abweichung Soll/Ist digitale Ausgänge
  }
  if (g == 1) {
    digitalWrite(ledPin1, HIGH);
    digitalWrite(ledPin2, LOW);
  }
  else {
    digitalWrite(ledPin1, LOW);
    digitalWrite(ledPin2, HIGH);
  }
}

//============

int getdigitalout(int device) {
  int val = 0;
  int i = 0;
  for (i = 0; i < nPins; i++) {
    val = val + (digitalRead(firstPins[device - 1] + i) << (i));
  }
  for (i = 0; i < len; i++) {
    if (oswPattern[i] == val) {
      break;
    }
  }
  if (i+1>len) { // kein oswPattern == val -> return -1
    i = -2;  
  }
  return i + 1;
}