/*!
 *
 *

                                      +-----+
         +----[PWR]-------------------| USB |--+
         |                            +-----+  |
         |         GND/RST2  [ ][ ]            |
         |       MOSI2/SCK2  [ ][ ]  A5/SCL[ ] |   C5 
         |          5V/MISO2 [ ][ ]  A4/SDA[ ] |   C4 
         |                             AREF[ ] |
         |                              GND[ ] |
         | [ ]N/C                    SCK/13[ ] |   HEART_LED
         | [ ]IOREF                 MISO/12[ ] |   .
         | [ ]RST                   MOSI/11[ ]~|   .
         | [ ]3V3    +---+               10[ ]~|   .
         | [ ]5v    -| A |-               9[ ]~|   .
         | [ ]GND   -| R |-               8[ ] |   B0
         | [ ]GND   -| D |-                    |
         | [ ]Vin   -| U |-               7[ ] |   D7
         |          -| I |-               6[ ]~|   .
         | [ ]A0    -| N |-               5[ ]~|   .
         | [ ]A1    -| O |-               4[ ] |   CLOCK_PIN
         | [ ]A2     +---+           INT1/3[ ]~|   LATCH_PIN
         | [ ]A3                     INT0/2[ ] |   DATA_PIN
         | [ ]A4/SDA  RST SCK MISO     TX>1[ ] |   .
         | [ ]A5/SCL  [ ] [ ] [ ]      RX<0[ ] |   D0
         |            [ ] [ ] [ ]              |
         |  UNO_R3    GND MOSI 5V  ____________/
          \_______________________/
		  
		  http://busyducks.com/ascii-art-arduinos

*/
#include "Arduino.h"
#include "Vrekrer_scpi_parser.h"
#include "FreqPeriodCounter.h"

/// SCPI *IDN? Responses

const String COPYRIGHT = "-1";      ///< Copyright company and year. \n\n Part of the *IDN? response. See identify().
const String UID = "-1";                        ///< UID
const String PRODUCT = "-1";                 ///< Project name. \n\n Part of the *IDN? response. See identify().
const String SOFTWAREREV = "-1";                  ///< Firmware version. \n\n Part of the *IDN? response. See identify().
int hardwareRev = -1;                             ///< Takes the value of the #VERSION_PINS. \n\n Part of the *IDN? response. See identify().
const String DIVIDER = ",\t";                     ///< String divider. \n\n Part of the *IDN? response. See identify().

/// Heartbeat LED
const int HEART_LED = 13;
bool heartBeatState = true;
int previousBeat = 0;
const int beatSpeed = 1;                      ///< Whole number in Hz

/// Relays

const byte DATA_PIN = 2;
const byte LATCH_PIN = 3;
const byte CLOCK_PIN = 4;
const byte RELAY_COUNT = 14;  ///< Count of how many relays their are in total.
bool relayState[RELAY_COUNT];  ///< Array for storing state of the relay array in.



/// SCPI

SCPI_Parser scpi;                                 ///<  loads the SCPI_Parser function in as scpi


/**
 * The standard Arduino setup function used for setup and configuration tasks.
 * 
 * Sets the pinmodes, 
 */
void setup() {
  pinMode(HEART_LED,OUTPUT);
  pinMode(DATA_PIN,OUTPUT);
  pinMode(LATCH_PIN,OUTPUT);
  pinMode(CLOCK_PIN,OUTPUT);

  initScpi();
  clearRelayState();
  Serial.begin(9600);
  Serial.println(" "); ///< Required to bringup the serial console in Wokwi
}

/**
 * @brief Loop
 * The standard Arduino loop function used for repeating tasks.
 * 
 * heartBeat()
 * SCPI management
 * relays()
 */
void loop() {
  heartBeat();
  scpi.ProcessInput(Serial, "\n");
  relays();
}

void(* resetFunc) (void) = 0; //declare reset function @ address 0

void initScpi() {
  scpi.RegisterCommand(F("*IDN?"), &identify);
  scpi.RegisterCommand(F("*OPT?"), &options);
  scpi.RegisterCommand(F("*RST"), &hardReset);
  scpi.RegisterCommand(F("RELAy#?"), &relayStatus);
}

/**
 * Respond to *IDN?
 * 
 * Returns the Manufacturer, hardware type, software version, and a serial number.
 * (This serial number is different to the one from the one of the USB/Serial converter.)
 * 
 * This value is made up of the followning: #COPYRIGHT, #PRODUCT, SW: #SOFTWAREREV, HW: #hardwareRev, SN: <UniqueID>.
 *
 * This is part of initScpi().
 */
void identify(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  interface.println(COPYRIGHT + DIVIDER + PRODUCT + DIVIDER + "SW:" + SOFTWAREREV + DIVIDER + "HW:" + hardwareRev + DIVIDER + "SN:" + UID);
}

/**
 * Respond to *OPT?
 */
void options(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  interface.println("Options - not implimented");
}

/**
 * Respond to *RST
 */
void hardReset(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  interface.println("Reset - not implimented");
}

/**
 * Relay Status
  */
void relayStatus(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  //RELAy<index>?

  //Get the numeric suffix/index (if any) from the commands
  String header = String(commands.Last());
  header.toUpperCase();
  int suffix = -1;
  sscanf(header.c_str(),"%*[DIN]%u", &suffix);

  //If the suffix is valid, print the relays logic state to the interface

// sizeof(relayState) + 1

}


/**
 * Sets the output chanels to the required state.
 */
void relays() {
  digitalWrite(LATCH_PIN, LOW);
  for (int i = 0; i < sizeof(relayState) + 1; i++) {
    // Writes the value of the array out.;
    digitalWrite(DATA_PIN, relayState[i]);
    // Toggle the clock
    digitalWrite(CLOCK_PIN, HIGH);
    digitalWrite(CLOCK_PIN, LOW);
  }
  // bring the latches back up;
  digitalWrite(LATCH_PIN, HIGH);
}

void clearRelayState() {
  for (int i = 0; i < sizeof(relayState) + 1; i++) {
    // Fills the value on the array with a zero, then writes that value out.
    relayState[i] = LOW;
  }
  relays();
}

/**
 * Flashes the #HEART_LED at #HEART_BEAT, This flashing is to confirm the processor is still running.
 */
void heartBeat() {
  if (millis() - previousBeat >= 500 / beatSpeed ) {
    // save the last time you blinked the LED
    previousBeat = millis();

    // if the LED is off turn it on and vice-versa:
    if (heartBeatState == false) {
      heartBeatState = true;
    } else {
      heartBeatState = false;
    }
    digitalWrite(HEART_LED, heartBeatState);
  }
}