#include <Arduino.h>
#include <OneWire.h>                // needed for DS18B20 temperature probe
#include <DallasTemperature.h>      // needed for DS18B20 temperature probe
#include <Wire.h>                   // needed for I2C
#include <LiquidCrystal_I2C1602.h>  // needed for LCD1602-I2C
//#include <Wire.h>                   // needed for I2C
#include <EEPROM.h>                   // needed for EEPROM
#include "eepromanything.h"           // needed for EEPROM

// #define EEPROMSIZE 512              // ATMEGA168 512 EEPROM
#define EEPROMSIZE 1024               // ATMEGA328P 1024 EEPROM

// these are stored in EEPROM - all variables in a structure
struct config_t {
  int validdata;       // if this is 99 then data is valid
  long fposition;       // last focuser position
  long maxstep;         // max steps
  int stepmode;                  // stepping mode
  // indicates stepmode, full, half, 1/4, 1/8. 1/16. 1/32 [1.2.4.8.16.32]
  boolean ReverseDirection;      // reverse direction
  boolean coilPwr;               // coil pwr
} myfocuser;

int datasize;      // will hold size of the struct myfocuser - 6 bytes
int nlocations;    // number of storage locations available in EEPROM
int currentaddr;   // will be address in eeprom of the data stored
boolean writenow;     // should we update values in eeprom
boolean found;        // did we find any stored values?
long previousMillis = 0L;   // used as a delay whenever the EEPROM settings need to be updated
long interval = 10000L;     // interval in milliseconds to wait after a move before writing settings to EEPROM, 10s
long numwrites = 0L;        // number of eeprom writes this session
float tempoffsetval = 0;    // used to calibrate temperature offset, moonlite focuser
int stepdelay = 2;          // used by moonlite focuser
int tempcomp = 0;           // used by moonlite focuser

const String programName = "FocusUser - 4000 MaxStep";
const String programVersion = "1.39 Mini";

// define temperature probe
#define ch1temp 2      // temperature probe on pin 2, use 4.7k pullup
// define objects for temperature probes
OneWire oneWirech1(ch1temp);            // setup temperature probe 1
// Pass our oneWire reference to Dallas Temperature library
DallasTemperature sensor1(&oneWirech1); // probe

// DB18B20 info
#define MaxProbes 2       // maximum probes per channel, really only supports 1
#define TEMP_PRECISION 12 // Set the DS18B20s precision (12 bit) 
DeviceAddress tpAddress;  // used to send precision setting to specific sensor
int tprobe1 = 0;          // indicate if there is a probe attached to that channel
float ch1tempval;         // temperature value for probe

// Stepper Motor stuff, control pins for DRV8825 board
#define myDir     3
#define myStep    4
#define myM0      5  // microstepping lines
#define myM1      6
#define myM2      7
// m0/m1/m2 sets stepping mode 000 = F, 100 = 1/2, 010 = 1/4, 110 = 1/8, 001 = 1/16, 101 = 1/32
// steps per revolution = 200, 400, 800, 1600, 6400
// must set the current limiting for microstepping to work correctly

// ENABLE - Setting CoilPwr controls the disable or enable state of the EASYDRIVER board outputs
int boardstate;
#define OUTPUTENABLED 1
#define OUTPUTDISABLED 0
#define myEnable  8        // For DRV8825 and Easydriver boards, wire /ENABLE pin to D8 input
// NOTE: If using microstepping, coil power should always be ON and do NOT disable the board

// stepontime - time in microseconds that coil power is ON for one step, board requires 2us pulse
int stepontime = 5;          // changed 20141230 - seems better when 1/4 and 1/8 stepping
// motorSpeed - time in milisecobds of delay between stepper pulses
int motorSpeedSlow = 40;         // for 1000 half steps = about 56s
int motorSpeedMed = 18;          // for 1000 half steps = about 36s
int motorSpeedFast = 2;          // for 1000 half steps = about 18s
int motorSpeed = motorSpeedMed;         // changed 20141230 - seems better when 1/4 and 1/8 stepping

// NOTE: Use Analogue pins
// define IN and OUT LEDS, associated with PB and stepper moves
#define bledIN A1
#define gledOUT A2
// define Buzzer
#define Buzzer A3

// Default initial positions if not set/overriden by Ascom Driver or Winapp
long currentPosition = 0L;   // current position
long targetPosition = 2000L;    // target position
long maxFocuserLimit = 4000L;  // arbitary focuser limit
long maxSteps = 4000L;         // maximum position of focuser
long maxIncrement = 1000L;      // maximum number of steps permitted in one move
long minimumPosition = 0L;      // minimum position to avoid focuser damage
boolean gotonewposition = false;  // used by moonlite after an SN command followed by a FG

char inChar;                  // used to read a character from serial port
boolean isMoving = false;     // is the motor currently moving
long pos;

#define MAXCOMMAND 8
char cmd[MAXCOMMAND];         // these are for handling and processing serial commands
char param[MAXCOMMAND];
char line[MAXCOMMAND];
int eoc = 0;    // end of command
int idx = 0;    // index into command string

LiquidCrystal_I2C lcd(0x27, 20, 4);    // connects to A4/A5
int loopcount = 0;                     // used to reduce LCD flicker during loop()
int lcdupdatepos = 0;                  // used to update lcd focuser positions during moves
#define LCDUPDATERATE 25000            // to stop flickering, 25000=1second, max value = 32766
boolean displayenabled = true;         // used to enable and disable the display

// read the push button switches and return state of switches
// 1 = SW1 ON AND SW2 OFF, 2 = SW2 ON AND SW1 OFF, 3 = SW1 ON and SW2 ON, 0 = OFF
int readpbswitches(int pinNum) {
  //  sw1 (681) 650-720, sw2 (338) 310-380, sw1 and sw2 (509) 460-530
  int val = 0;                       // variable to store the read value
  digitalWrite((14 + pinNum), HIGH); // enable 20k internal pullup, 14=A0
  val = analogRead(pinNum);          // read the input pin
  if ( val >= 650 && val <= 720 ) {
    return 1;                        // toggle sw1 ON and SW2 OFF
  }
  else if ( val >= 460 && val <= 530 ) {
    return 3;                    // toggle sw1 and sw2 ON
  }
  else if ( val >= 310 && val <= 380 ) {
    return 2;                    // toggle sw2 ON and SW1 OFF
  }
  else return 0;                   // switches are OFF
}

// disable the stepper motor outputs - coil power off
void disableoutput() {
  digitalWrite(myEnable, HIGH);
  // control via ENABLE pin, but this turns off indexer inside DRV8825
  // which means the stepper motor will loose power and move position to the nearest full step
  // not an issue if using full steps but major issue if using microstepping as will cause change
  // in focus position
}

// enable the stepper motor outputs - coil power on
void enableoutput() {
  digitalWrite(myEnable, LOW);
}

// Move stepper anticlockwise
void anticlockwise() {
  if ( !myfocuser.ReverseDirection ) {
    digitalWrite(gledOUT, HIGH ); // indicate a pulse by lighting the green LED
    digitalWrite(myDir, LOW );
    digitalWrite(myStep, 1 );
    delayMicroseconds(stepontime);
    digitalWrite(myStep, 0 );
    digitalWrite(gledOUT, LOW ); // turn LED off
  }
  else {
    digitalWrite(bledIN, HIGH );  // indicate a pulse by lighting the blue LED
    digitalWrite(myDir, HIGH );
    digitalWrite(myStep, 1 );
    delayMicroseconds(stepontime);
    digitalWrite(myStep, 0 );
    digitalWrite(bledIN, LOW );  // turn LED off
  }
}

// Move stepper clockwise
void clockwise() {
  if ( !myfocuser.ReverseDirection ) {
    digitalWrite(bledIN, HIGH );  // indicate a pulse by lighting the blue LED
    digitalWrite(myDir, HIGH );
    digitalWrite(myStep, 1 );
    delayMicroseconds(stepontime);
    digitalWrite(myStep, 0 );
    digitalWrite(bledIN, LOW );  // turn LED off
  }
  else {
    digitalWrite(gledOUT, HIGH ); // indicate a pulse by lighting the green LED
    digitalWrite(myDir, LOW );
    digitalWrite(myStep, 1 );
    delayMicroseconds(stepontime);
    digitalWrite(myStep, 0 );
    digitalWrite(gledOUT, LOW ); // turn LED off
  }
}

// set the microstepping mode
void setstepmode() {
  switch ( myfocuser.stepmode ) {
    case 1:      // full step
      digitalWrite(myM0, 0);
      digitalWrite(myM1, 0);
      digitalWrite(myM2, 0);
      break;
    case 2:      // half step
      digitalWrite(myM0, 1);
      digitalWrite(myM1, 0);
      digitalWrite(myM2, 0);
      break;
    case 4:
      digitalWrite(myM0, 0);
      digitalWrite(myM1, 1);
      digitalWrite(myM2, 0);
      break;
    case 8:
      digitalWrite(myM0, 1);
      digitalWrite(myM1, 1);
      digitalWrite(myM2, 0);
      break;
    case 16:
      digitalWrite(myM0, 0);
      digitalWrite(myM1, 0);
      digitalWrite(myM2, 1);
      break;
    case 32:
      digitalWrite(myM0, 1);
      digitalWrite(myM1, 0);
      digitalWrite(myM2, 1);
      break;
  }
}
 
// ASCOM Serial Commands
void processCommand(String command) {
  memset(cmd, 0, MAXCOMMAND);
  memset(param, 0, MAXCOMMAND);
  int len = strlen(line);
  if (len >= 2) {
    strncpy(cmd, line, 2);
  }
  if (len > 2) {
    strncpy(param, line + 2, len - 2);
  }

  memset(line, 0, MAXCOMMAND);
  eoc = 0;
  idx = 0;

  // get the current focuser position
  if (!strcasecmp(cmd, "GP")) {
    char tempString[6];
    sprintf(tempString, "%04X", currentPosition);
    Serial.print(tempString);
    Serial.print("#");
  }

  // motor is moving - 1 if moving, 0 otherwise
  else if (!strcasecmp(cmd, "GI")) {
    if (isMoving ) {
      Serial.print("01#");
    }
    else {
      Serial.print("00#");
    }
  }

  // initiate a move to the target position
  else if (!strcasecmp(cmd, "FG")) {
    gotonewposition = true;
    isMoving = true;
  }

  // stop a move - HALT
  else if (!strcasecmp(cmd, "FQ")) {
    gotonewposition = false;
    isMoving = false;
    targetPosition = currentPosition;
  }

  // home the motor, hard-coded, ignore parameters
  // not implemented in INDI driver
  else if (!strcasecmp(cmd, "PH")) {
    gotonewposition = true;
    isMoving = true;
    targetPosition = 0;
  }

  // get the new motor position (target)
  // not implemented in INDI driver
  else if (!strcasecmp(cmd, "GN")) {
    char tempString[6];
    sprintf(tempString, "%04X", targetPosition);
    Serial.print(tempString);
    Serial.print("#");
  }

  // set fullstep mode
  else if (!strcasecmp(cmd, "SF")) {
    myfocuser.stepmode = 1;
    setstepmode();
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start time interval
  }

  // set halfstep mode
  else if (!strcasecmp(cmd, "SH")) {
    myfocuser.stepmode = 2;
    setstepmode();
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start time interval
  }

  // whether half-step is enabled or not, moonlite always return "00"
  else if (!strcasecmp(cmd, "GH")) {
    if ( myfocuser.stepmode == 2 )
      Serial.print("FF#");
    else
      Serial.print("00#");
  }

  // get stepmode
  // myfocuser command
  else if (!strcasecmp(cmd, "GS")) {
    char tempString[6];
    sprintf(tempString, "%02X", myfocuser.stepmode);
    Serial.print(tempString);
    Serial.print("#");
  }

  // set stepmode
  // myfocuser command
  else if (!strcasecmp(cmd, "SS")) {
    pos = hexstr2long(param);
    myfocuser.stepmode = pos;
    setstepmode();
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start time interval
  }

  // set current position to received position - no move SPXXXX
  // in INDI driver, only used to set to 0 SP0000 in reset()
  else if (!strcasecmp(cmd, "SP")) {
    pos = hexstr2long(param);
    if ( pos > maxSteps ) pos = maxSteps;
    if ( pos < 0 ) pos = 0;
    currentPosition = pos;
    targetPosition = pos;
    // signal that the focuser position has changed and should be saved to eeprom
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start 30s time interval
    gotonewposition = false;
    isMoving = false;
  }

  // set new target position SNXXXX - this is a move command
  // but must be followed by a FG command to start the move
  else if (!strcasecmp(cmd, "SN")) {
    pos = hexstr2long(param);
    if ( pos > maxSteps ) pos = maxSteps;
    if ( pos < 0 ) pos = 0;
    targetPosition = pos;
    gotonewposition = false;
    isMoving = false;
  }

  // get the current temperature - moonlite compatible
  else if (!strcasecmp(cmd, "GT")) {
    // no probe so just return 20
    char tempString[6];
    // Send the command to get temperatures from DS18B20 probes
    if ( tprobe1 == 1 ) {
      sensor1.requestTemperatures();
      delay(750 / (1 << (12 - TEMP_PRECISION))); // should enough time to wait
      // get channel 1 temperature
      if ( tprobe1 == 1 ) ch1tempval = sensor1.getTempCByIndex(0);
    }
    else ch1tempval = 20;      // default temp if there is no probe
    ch1tempval = (ch1tempval * 2) + tempoffsetval;
    sprintf(tempString, "%04X", (int) ch1tempval);
    Serial.print(tempString);
    Serial.print("#");
  }

  // get the current temperature
  // myfocuser command
  else if (!strcasecmp(cmd, "GZ")) {
    // Send the command to get temperatures from DS18B20 probes
    if ( tprobe1 == 1 ) {
      sensor1.requestTemperatures();
      delay(750 / (1 << (12 - TEMP_PRECISION))); // should enough time to wait
      // get channel 1 temperature
      if ( tprobe1 == 1 ) ch1tempval = sensor1.getTempCByIndex(0);
    }
    else ch1tempval = 20;      // default temp if there is no probe
    Serial.print(ch1tempval);
    Serial.print("#");
  }

  // firmware value Moonlite
  else if (!strcasecmp(cmd, "GV")) {
    Serial.print("10#");
  }

  // firmware value
  // myfocuser command
  else if (!strcasecmp(cmd, "GF")) {
    Serial.println(programName);
    Serial.print(programVersion + "#");
  }

  // get the current motor step delay, only values of 02, 04, 08, 10, 20
  // not used so just return 02, moonlite only
  else if (!strcasecmp(cmd, "GD")) {
    char tempString[6];
    sprintf(tempString, "%02X", stepdelay);
    Serial.print(tempString);
    Serial.print("#");
  }

  // set step delay, only acceptable values are 02, 04, 08, 10, 20 which
  // correspond to a stepping delay of 250, 125, 63, 32 and 16 steps
  // per second respectively. Moonlite only
  else if (!strcasecmp(cmd, "SD")) {
    pos = hexstr2long(param);
    stepdelay = (int) pos;
  }

  // set temperature co-efficient XX
  else if (!strcasecmp(cmd, "SC")) {
    // do nothing, ignore
    pos = hexstr2long(param);
    tempcomp = (int) pos;
  }

  // get temperature co-efficient XX
  else if (!strcasecmp(cmd, "GC")) {
    // do nothing, ignore
    char tempString[6];
    sprintf(tempString, "%02X", tempcomp);
    Serial.print(tempString);
    Serial.print("#");
  }

  // + activate temperature compensation focusing
  else if (!strcasecmp(cmd, "+")) {
    // ignore
  }

  // - disable temperature compensation focusing
  else if (!strcasecmp(cmd, "-")) {
    // ignore
  }

  // temperature calibration offset POXX in 0.5 degree increments (hex)
  else if (!strcasecmp(cmd, "PO")) {
    // Moonlite only
    // this adds/subtracts an offset from the temperature reading in 1/2 degree C steps
    // FA -3, FB -2.5, FC -2, FD -1.5, FE -1, FF -.5, 00 0, 01 0.5, 02 1.0, 03 1.5, 04 2.0, 05 2.5, 06 3.0
    tempoffsetval = 0.0;
    if( param == "FA" ) tempoffsetval = -3.0;
    else if ( param == "FB") tempoffsetval = -2.5;
    else if ( param == "FC") tempoffsetval = -2.0;
    else if ( param == "FD") tempoffsetval = -1.5;
    else if ( param == "FE") tempoffsetval = -1.0;
    else if ( param == "FF") tempoffsetval = -0.5;
    else if ( param == "00") tempoffsetval = 0.0;
    else if ( param == "01") tempoffsetval = 0.5;
    else if ( param == "02") tempoffsetval = 1.0;
    else if ( param == "03") tempoffsetval = 1.5;
    else if ( param == "04") tempoffsetval = 2.0;
    else if ( param == "05") tempoffsetval = 2.5;
    else if ( param == "06") tempoffsetval = 3.0;
  }
  
  // set new maxSteps position SMXXXX
  // myfocuser command
  else if (!strcasecmp(cmd, "SM")) {
    pos = hexstr2long(param);
    if ( pos > maxFocuserLimit ) pos = maxFocuserLimit;
    // avoid setting maxSteps too low
    if ( pos < 1000 ) pos = 1000;
    // for NEMA17 at 400 steps this would be 5 full rotations of focuser knob
    // for 28BYG-28 this would be less than 1/2 a revolution of focuser knob
    maxSteps = pos;
    // signal that the focuser position has changed and should be saved to eeprom
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start 30s time interval
  }

  // get the MaxSteps
  // myfocuser command
  else if (!strcasecmp(cmd, "GM")) {
    char tempString[6];
    sprintf(tempString, "%04X", maxSteps);
    Serial.print(tempString);
    Serial.print("#");
  }

  // set new maxIncrement SYXXXX
  // myfocuser command
  else if (!strcasecmp(cmd, "SY")) {
    pos = hexstr2long(param);
    // ignore this and just set it to MaxSteps as the maximum move allowed
    pos = maxSteps;
    maxIncrement = pos;
  }

  // get the maxIncrement - set to MaxSteps
  // myfocuser command
  else if (!strcasecmp(cmd, "GY")) {
    char tempString[6];
    sprintf(tempString, "%04X", maxSteps);
    Serial.print(tempString);
    Serial.print("#");
  }

  // get the coilPwr setting
  // myfocuser command
  else if (!strcasecmp(cmd, "GO")) {
    String tempString;
    if ( myfocuser.coilPwr )
      tempString = "01#";
    else
      tempString = "00#";
    Serial.print(tempString);
  }

  // set the coilPwr setting
  // myfocuser command
  else if (!strcasecmp(cmd, "SO")) {
    pos = hexstr2long(param);
    if ( pos == 0 )
      myfocuser.coilPwr = false;
    else
      myfocuser.coilPwr = true;
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start time interval
  }

  // get the Reverse Direction setting
  // myfocuser command
  else if (!strcasecmp(cmd, "GR")) {
    String tempString;
    if ( myfocuser.ReverseDirection )
      tempString = "01#";
    else
      tempString = "00#";
    Serial.print(tempString);
  }

  // set the Reverse Direction setting
  // myfocuser command
  else if (!strcasecmp(cmd, "SR")) {
    pos = hexstr2long(param);
    if ( pos == 0 )
      myfocuser.ReverseDirection = false;
    else
      myfocuser.ReverseDirection = true;
    writenow = true;             // updating of EEPROM ON
    previousMillis = millis();   // start time interval
  }

  // set motorSpeed - time delay between pulses, acceptable values are 00, 01 and 02 which
  // correspond to a slow, med, high
  // myfocuser command
  else if (!strcasecmp(cmd, "MS")) {
    pos = hexstr2long(param);
    if ( pos == 0 )
      motorSpeed = motorSpeedSlow;
    else if ( pos == 1 )
      motorSpeed = motorSpeedMed;
    else if (pos == 2 )
      motorSpeed = motorSpeedFast;
    else
      motorSpeed = motorSpeedMed;
  }

  // LED backlight value, always return "00" - moonlite
  // not implemented in INDI driver
  else if (!strcasecmp(cmd, "GB")) {
    Serial.print("00#");
  }

  else if (!strcasecmp(cmd, "DS")) {  // disable or enable the display setting
    pos = hexstr2long(param);
    if ( pos == 0 ) 
    {
      // disable the screen by setting clearing the screen and then the text color to black
      // clear the screen
      lcd.noDisplay();
      lcd.noBacklight();
      displayenabled = false;
    }
    else if ( pos == 1 )
    {
      // set the text color to white
      lcd.display();
      lcd.backlight();
      displayenabled = true;
    }
  }
  else if(!strcasecmp(cmd, "DG")) {
    if( displayenabled == true )
      Serial.print("01#");
    else
      Serial.print("00#");
  }
  
  // troubleshooting only - print currentaddr value, use in serial monitor mode is best
  else if (!strcasecmp(cmd, "XY")) {
    Serial.println();
    Serial.print("Current EEPROM Addr= ");
    Serial.println(currentaddr);
    Serial.print("nlocations = ");
    Serial.println(nlocations);
    Serial.print("Number of writes= ");
    Serial.println(numwrites);
    Serial.print("focuser=");
    Serial.println(myfocuser.fposition);
    Serial.print("MaxSteps=");
    Serial.println(myfocuser.maxstep);
    Serial.print("Stepmode=");
    Serial.println(myfocuser.stepmode);
    Serial.print("CoilPower=");
    Serial.println(myfocuser.coilPwr);
    Serial.print("ReverseDirection=");
    Serial.println(myfocuser.ReverseDirection);
    Serial.print("Motorspeed=");
    Serial.println(motorSpeed);
    Serial.print("#");
  }

  // troubleshooting only - reset focuser defaults
  else if (!strcasecmp(cmd, "XZ")) {
    currentaddr = 0;
    myfocuser.validdata = 99;
    myfocuser.maxstep = 4000;
    myfocuser.fposition = 0;
    // now write the data to EEPROM
    EEPROM_writeAnything(currentaddr, myfocuser);    // update values in EEPROM
    numwrites++;
    // Set focuser defaults.
    currentPosition = myfocuser.fposition;
    targetPosition = myfocuser.fposition;
    maxSteps = myfocuser.maxstep;
    myfocuser.coilPwr = true;
    myfocuser.ReverseDirection = false;
    myfocuser.stepmode = 1;
  }
}

// convert hex string to long int
long hexstr2long(char *line) {
  long ret = 0;

  ret = strtol(line, NULL, 16);
  return (ret);
}

// displaylcd
void displaylcd() {
  // display values on LCD1602
  lcd.clear();
  lcd.print("P=");
  lcd.print(currentPosition);
  lcd.setCursor( 11, 0);
  lcd.print("G=");
  lcd.print(currentPosition);
  lcd.setCursor( 0, 1);
  lcd.print("T=");
  lcd.print(ch1tempval);
  lcd.setCursor( 0, 2 );
  lcd.print("Pwr=");
  if ( myfocuser.coilPwr ) lcd.print("ON"); else lcd.print("OFF");
  lcd.setCursor( 11, 2 );
  lcd.print("RD=");
  if ( myfocuser.ReverseDirection ) lcd.print("ON"); else lcd.print("OFF");
  lcd.setCursor( 0, 3 );
  lcd.print("Speed=");
  if( myfocuser.stepmode == 1 )
    lcd.print("Full");
  else if( myfocuser.stepmode == 2 )
    lcd.print("Half");
  else if( myfocuser.stepmode == 4 )
    lcd.print("1/4");
  else if( myfocuser.stepmode == 8 )
    lcd.print("1/8");
  else if( myfocuser.stepmode == 16 )
    lcd.print("1/16");
  else if( myfocuser.stepmode == 32 )
    lcd.print("1/32");
  loopcount = 0; 
}

// updatefposlcd
void updatefposlcd() {
  // display focuser position values on LCD1602
  lcd.setCursor( 2, 0);
  lcd.print(currentPosition);
  lcd.setCursor( 13, 0);
  lcd.print(targetPosition);
  lcdupdatepos = 1;
}

// Setup
void setup() {
  // initialize serial for ASCOM
  Serial.begin(9600);


  pinMode( Buzzer, OUTPUT );
  pinMode( bledIN, OUTPUT );
  pinMode( gledOUT, OUTPUT );
  
digitalWrite( Buzzer, HIGH );  // turn ON the Buzzer - provide power ON beep
  eoc = 0;
  idx = 0;
  isMoving = false;
  gotonewposition = false;
  memset(line, 0, MAXCOMMAND);

lcd.init();             // initialise the lcd display
  lcd.backlight();        // enable the backlight
displayenabled = true;

  // print the title of program and copyright message to lcd
  lcd.clear();
  lcd.print(programName);
  lcd.setCursor(0, 1);
  lcd.print(programVersion);

  pinMode(  myDir, OUTPUT );
  pinMode(  myStep, OUTPUT );
  pinMode(  myM0, OUTPUT );
  pinMode(  myM1, OUTPUT );
  pinMode(  myM2, OUTPUT );

  // set direction and step to low
  digitalWrite( myDir, 0 );
  digitalWrite( myStep, 0 );

  boardstate = OUTPUTENABLED;
  pinMode( myEnable, OUTPUT );    // enable the driver board
  enableoutput();

  currentaddr = 0;    // start at 0 if not found later
  found = false;
  writenow = false;
  datasize = sizeof( myfocuser );    // should be 14 bytes
  nlocations = EEPROMSIZE / datasize;  // for AT328P = 1024 / datasize = 73 locations
  numwrites = 0;

  for (int lp1 = 0; lp1 < nlocations; lp1++ ) {
    int addr = lp1 * datasize;
    EEPROM_readAnything( addr, myfocuser );
    // check to see if the data is valid
    if ( myfocuser.validdata == 99 ) {
      // data was erased so write some default values
      currentaddr = addr;
      found = true;
    }
  }
  if ( found == true ) {
    // set the focuser back to the previous settings
    // done after this in one hit
    // mark current eeprom address as invalid and use next one
    // each time focuser starts it will read current storage, set it to invalid, goto next location and
    // write values to there and set it to valid - so it doesnt always try to use same locations over and
    // over and destroy the eeprom
    // using it like an array of [0-nlocations], ie 100 storage locations for 1k EEPROM
    EEPROM_readAnything( currentaddr, myfocuser );
    myfocuser.validdata = 0;
    EEPROM_writeAnything(currentaddr, myfocuser);    // update values in EEPROM
    numwrites++;
    // goto next free address and write data
    currentaddr += datasize;
    // bound check the eeprom storage and if greater than last index [0-EEPROMSIZE-1] then set to 0
    if ( currentaddr >= (nlocations * datasize) ) currentaddr = 0;

    myfocuser.validdata = 99;
    EEPROM_writeAnything(currentaddr, myfocuser);    // update values in EEPROM
    numwrites++;
  }
  else {
    // set defaults because not found
    myfocuser.validdata = 99;
    myfocuser.maxstep = 10000L;
    myfocuser.fposition = 5000L;
    myfocuser.coilPwr = true;
    myfocuser.ReverseDirection = false;
    myfocuser.stepmode = 1;    // full stepping
    // now write the data to EEPROM
    EEPROM_writeAnything(currentaddr, myfocuser);    // update values in EEPROM
    numwrites++;
  }

  // Set focuser defaults from saved values in EEPROM.
  currentPosition = myfocuser.fposition;
  targetPosition = myfocuser.fposition;
  maxSteps = myfocuser.maxstep;

  // these will be updated by ASCOM driver or Applciation when connect()
  // these will be updated by ASCOM driver or Applciation when connect()
  // myfocuser.coilPwr
  // myfocuser.ReverseDirection
  // myfocuser.stepmode
  setstepmode();

  tempoffsetval = 0;
 
  loopcount = 0;      // set lcd display loop delay for focuser pos whilst not moving
  lcdupdatepos = 0;   // set lcd display loop delay for focuser pos whilst moving
 
 // start temperature sensors DS18B20
  tprobe1 = 0;        // set probe indicator NOT FOUND
  sensor1.begin();    // start the temperature sensor1
  sensor1.getDeviceCount();    // should return 1 if probe connected
  for (int i = 0; i < MaxProbes; i++) {  // look for probes
    // Search the wire for address
    if (sensor1.getAddress(tpAddress, i)) { // found?
      tprobe1 = 1;  // there is a probe1
      sensor1.setResolution(tpAddress, TEMP_PRECISION);
      // set the resolution for that probe to 10bit 0.25degC
    }
  }
// setup the IN/OUT LEDS
  digitalWrite( bledIN, LOW );    // turn off the direction LEDS
  digitalWrite( gledOUT, LOW );
   
  // end the power on buzzer beep
  digitalWrite( Buzzer, LOW);
}

// Main Loop
void loop() {
// lcd flickers if nothing happens, so use a counter
  loopcount++;
  if ( loopcount > LCDUPDATERATE ) {
    displaylcd();
    loopcount = 0;
  }
   
// process the command string when a hash arrives:
  if (eoc) {
    processCommand(line);
    memset(line, 0, MAXCOMMAND);
    eoc = false;
  }

  // Move the position by a single step if target <> current position
  if ((targetPosition != currentPosition) && (gotonewposition == true)) {
    if ( boardstate == OUTPUTDISABLED )       // if board is not enabled, we need to enable it else
    { // it will not step
      boardstate = OUTPUTENABLED;
      enableoutput();              // have to enable driver board
    }
    isMoving = true;              // focuser is moving
    writenow = true;              // updating of EEPROM ON
    previousMillis = millis();    // keep updating previousMillis whilst focuser is moving

 //   updatefposlcd();              // display the new current and target focuser positions
    delay( motorSpeed );          // delay between pulses to stepper thus controlling speed


    // Going Anticlockwise to lower position
    if (targetPosition < currentPosition) {
      anticlockwise();
      currentPosition--;
    }

    // Going Clockwise to higher position
    if (targetPosition > currentPosition) {
      // do not need to check if > maximumPosition as its done when a target command is receieved
      clockwise();
      currentPosition++;
    }
  } else {
    // focuser is NOT moving now, move is completed
    isMoving = false;
    gotonewposition = false;
    if ( writenow == true ) {    // time to update eeprom

      // decide if we have waited 10s after the last move, if so, update the EEPROM
      unsigned long currentMillis = millis();
      if ( (currentMillis - previousMillis > interval) && (writenow == true) ) {
        previousMillis = currentMillis;    // update the timestamp

        // copy current settings and write the data to EEPROM
        myfocuser.validdata = 99;
        myfocuser.fposition = currentPosition;
        myfocuser.maxstep = maxSteps;
        EEPROM_writeAnything(currentaddr, myfocuser);    // update values in EEPROM
        numwrites++;
        writenow = false;
      }
    }
    if ( myfocuser.coilPwr == false ) {
      boardstate = OUTPUTDISABLED;
      disableoutput();      // release the stepper coils to save power
    }
  }
}

// SerialEvent occurs whenever new data comes in the serial RX.
void serialEvent() {
  // : starts the command, # ends the command, do not store these in the command buffer
  // read the command until the terminating # character
  while (Serial.available() && !eoc) {
    inChar = Serial.read();
    if (inChar != '#' && inChar != ':') {
      line[idx++] = inChar;
      if (idx >= MAXCOMMAND) {
        idx = MAXCOMMAND - 1;
      }
    }
    else {
      if (inChar == '#') {
        eoc = 1;
      }
    }
  }
}
