/* Servo test
  by Don O
  March 16 2024
  This code created to test the servo library for another project that manages temperature control with a servo.
  The code receives a specific input (I used a 1 for simplicity in the test) and if it is the correct input, calls temp_Set() function
  temp_Set reads the memory value and if in range, maps the memory value to an angl esweep between 20 and 180 and then writes to the servo motor
  If successful, return success and if not, sends error from temp_Set() back to calling function
*/

#include <Servo.h>
#include <SafeString.h>
#include "SafeStringReader.h"

// create an sfReader instance of SafeStringReader class
// delimited by space, comma or CarrageReturn or NewLine
// the createSafeStringReader( ) macro creates both the SafeStringReader (sfReader) and the necessary SafeString that holds input chars until a delimiter is found
// args are (ReaderInstanceName, expectedMaxCmdLength, delimiters)
createSafeStringReader(sfReader, 32, " ,\r\n");

cSF(myRcvString, 32);
cSF(myPrintStr, 32);

boolean receivedBytesAndDelimiter;

const byte myServoPin = 11;

byte mem = 19;
byte tempPos = 0;

Servo tempServo;

void setup() {
  Serial.begin(9600);
  Serial.println("Setup-Start");
  tempServo.attach(myServoPin);   // attaches the servo object
  Serial.println();
  Serial.println(F(" Set the Arduino IDE monitor to Newline, or Carriage return or Both NL & CR"));
  Serial.println(F(" See the SafeStringReader_CmdsTimed.ino for an example using a struct to hold the commands and their functions."));

  SafeString::setOutput(Serial); // enable error messages and SafeString.debug() output to be sent to Serial
  sfReader.connect(Serial); // where SafeStringReader will read from
  //sfReader.echoOn(); // echo back all input, by default echo is off
}

void loop() {

  receivedBytesAndDelimiter = sfReader.read();

  if (receivedBytesAndDelimiter) {
    myRcvString = sfReader;
    Serial.print("myRcvString#");
    Serial.print(myRcvString);
    Serial.println("#");
  }

  if (myRcvString == "1") {
    myRcvString = "";
    if ( (mem >= 19) && (mem < 29) ) {
      //Increase temperature by 1 degree.
      mem++;
      myPrintStr  = "mem=";
      myPrintStr += mem;

      Serial.println(myPrintStr);
      temp_Set();
    }
    else {
      Serial.println("mem not between 19 ans 29");
    }
  }
  else {
    //Serial.println("not entering Up function");
  }

}


void temp_Set() {
  // Step the motor in increments by 1 degree:
  if ( (mem >= 19) && (mem <= 30) ) {
    //increment temperature by 1 degree and servo position
    tempPos = map(mem, 19, 30, 20, 180);
    //Move temp servo by mapped increment
    tempServo.write(tempPos);
    Serial.println("tempPos = " + String(tempPos));
    Serial.println("success in Temp_Set");
  }
  else {
    Serial.println("Err temp_Set");
  }
}