#include <Servo.h>
#include <Arduino.h>
#include <Vrekrer_scpi_parser.h>
#include "ArduinoUniqueID.h"

/// Servos
#define NUM_SERVOS 12
Servo myServo[NUM_SERVOS];
int Servo_Home[NUM_SERVOS];

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


/// These are found and replaced by the compiler.
#define CompileDate __DATE__ 
#define CompileTime __TIME__

/// These are for returning copyright information, and storing this information in the code.
const String Copyright = "Philip McGaw"; ///< Copyright company. Part of the *IDN? response. See identify().
String Product = "12 Servos SCPI";       ///< Project name. Part of the *IDN? response. See identify().
int Hardware_Rev = -1;                   ///< DO NOT change this number. Takes the value of the #VERSION_PINS. Part of the *IDN? response. See identify().
String Serial_Number = "-1";             ///< DO NOT change this number.This is the serial number of the microprocessor.
const int Software_Version = 1;          ///< Firmware version. Part of the *IDN? response. See identify().

String CompileYear = CompileDate;



void setup() 
{
  identification();                     ///< Generates the response for the *IDN? command.
  scpi_tree();                          ///< registers the SCPI commands

  Serial.begin(9600);
  Serial.println(" ");                ///< Opens the serial console in Wokwi

  // Attach the 12 servos
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].attach( i + 2);        ///< Attach the servos to pins D2 - D13 (D0 and D1 are used for the serial port)
    Servo_Home[i] = 90;                          ///< Sets the home position to 90 (horn is in middle positon)
    myServo[i].write( Servo_Home[i]);            ///< set all servos to Home position 
    Servo_Home[i] = 180;
  }

}

void loop() 
{
  scpi.ProcessInput(Serial, "");
}

/**
  Sets up the Tree for the SCPI commands

  Four Default ones are:

  *IDN?   Identify    Outputs an identifying string. The response will show the following information:
                      <company name>, <model number>, <serial number>, <firmware revision>
  *OPT?   Options     Returns a comma-separated list of all the instrument options currently installed on the equipment.
 */
void scpi_tree() {
  scpi.RegisterCommand(F("*IDN?"), &Identify);
  scpi.RegisterCommand(F("*OPT?"), &Options);
  scpi.SetCommandTreeBase("SET"); 
    scpi.RegisterCommand(F(":HOMe#"), &SetHome);
    scpi.RegisterCommand(F(":HOMe#?"), &GetHome);
  scpi.RegisterCommand(F("GOhome"), &HomeServos);
}

void HomeServos(SCPI_C commands, SCPI_P parameters, Stream& interface)
{
  //GOhome
  // sends all servo's to the home positions
  for( int i=0; i<NUM_SERVOS; i++)
  {
    myServo[i].write( Servo_Home[i]);            ///< set servos to Home position 
  }
}

void GetHome(SCPI_C commands, SCPI_P parameters, Stream& interface)
{
  //HOme<index>?
  //Queries the home position of the servo
  //Return values are an intergrer between 0 - 180
  //Examples:
  // SET:HOMe4?    (Queries the state of Servo[4])

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

  //If the suffix is valid, print the pin's logic value to the interface
  if ( (suffix >= 0) && (suffix < NUM_SERVOS) ) 
  {
    interface.println(Servo_Home[suffix]);
  }
}

void SetHome(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  //HOme<index> state
  //Sets the home position of the servo
  //Valid states are: 0 - 180
  //Examples:
  // SET:HOme4 180   (Sets Servo[4]'s home position to 180)


  //Get the numeric suffix/index (if any) from the commands
  String header = String(commands.Last());
  header.toUpperCase();
  int suffix = -1;
  sscanf(header.c_str(),"%*[HOME]%u", &suffix);
  //If the suffix is valid,
  //use the first parameter (if valid) to set the digital Output
  int first_parameter = int(parameters.First());
  // first_parameter.toUpperCase(); Redundent as the first_parameter is an int…
  if ( (suffix >= 0) && (suffix < NUM_SERVOS) ) 
  {
    if ( (first_parameter >= 0 ) && (first_parameter <= 180 ))
    {
      Servo_Home[suffix] = first_parameter ;
    }
  interface.println("Set:Home");
  }
}



/**
 * 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>.
 *
 */
void Identify(SCPI_C commands, SCPI_P parameters, Stream& interface) {
  interface.println((String) "© " + Copyright + " " + CompileYear + " - " + Product);
  interface.println((String) "Software Version: \t\t" + Software_Version + " (" + CompileDate + " " + CompileTime + ")");
  interface.println((String) "Hardware Version: \t\t" + Hardware_Rev + " (Known issue in Wokwi)");
  interface.println((String) "Serial Number: \t\t\t" + Serial_Number);
}

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

/**
 * Works out the hardware version, and the Serial number of the microcontroller.
 *
 * This code is run once during setup.
 */
void identification() {
  /**
    Resistor Values:
      Version	  Ratio VDD (dec)   Top Resistor Value		Bottom Resistor Value
      0	    	  1/16 (0.0625)  	  4k7		        				DNF
      1		      3/16 (0.1875)	    910R					        3k9
      2		      5/16 (0.3125)	    1k5						        3k3
      3		      7/16 (0.4375)	    2k4						        2k4 & 430R
      4		      9/16 (0.5625)	    2k4 & 430R	          2K4
      5		      11/16 (0.6875)	  3k3						        1k5
      6		      13/16 (0.8125)	  3k9						        910R
      7		      15/16 (0.9357)	  DNF						        4k7
  */
  int valA = analogRead(PIN_A0);  // Analog pin A1
  int valB = analogRead(PIN_A1);  // Analog  pin A0
  valA = map(valA, 0, 1023, 7, 0); // Maps the value from analouge pin to 0-7.
  valB = map(valB, 0, 1023, 7, 0);

  if (valB != 0)
    valB = valB + 7;          // adds 7 to the MSB if it is not zero
  Hardware_Rev = valA; + valB; // gives us a hardware version between 0 and 21.

  Serial_Number = "";  ///< empties the string removing the -1 value we filled it with,

  for (size_t i = 0; i < UniqueIDsize; i++) {
    Serial_Number += String(UniqueID[i], HEX);
  }
  Serial_Number.toUpperCase();

  CompileYear = CompileYear.substring(7, 11); ///< cleans up the date to just the year, used on the copyright string. Part of the *IDN? response. See identify().
}