/**************************************************************************

This is an example for our Monochrome OLEDs based on SSD1306 drivers

OLED 128x64 pixel display using I2C to communicate

Universal 8bit Graphics Library (https://github.com/olikraus/u8g2/)

Copyright (c) 2016, [email protected]
All rights reserved.

**************************************************************************/

//////////////////////////////////////////////////////////////////////////////
/// \file
/// \author    Lasse Toimela (original cmfw 0.9)
/// \author    Matti Ristimäki, Ville Vuorio (oled_LCB2_v9.0E)
/// \copyright Planmeca Oy
/// \date      2023
/// NOTICE     Big variable naming change done in v4.0. All Z_AXEL related names changed to Z_AXEL.
//////////////////////////////////////////////////////////////////////////////

#include <avr/io.h>
#include <U8g2lib.h>
#include <Wire.h>



#define VERSION_MAJOR 9
#define VERSION_MINOR 0
// Lisää kirjain riville 125.



/*
//#define DEBUG

#define DIR_UP 1
#define DIR_DOWN (-1)
#define DIR_STOP 0

//#define OSCILLATOR_FREQ 4e6f
#define OSCILLATOR_FREQ 4000000l

#define HOMING_COMPARE_OFFSET 4095l        // changed from 1000l to 4095l
#define HOMING_COMPARE_OFFSET_SLOW 30000l  // created a new variable for Z-AXEL homing speed, which will tilt Z-AXEL slowly when homed.


static long Z_AXEL_MAX_SPEED = 0;

#define MOTOR_RESET_PIN 10  // Needs to go UP

#define HEARTBEAT_LED 13


#define Z_AXEL_ENABLE_PIN 3
#define Z_AXEL_STEP_PIN 2

#define Z_AXEL_LIMIT_PIN_UPPER 14
#define Z_AXEL_LIMIT_PIN_LOWER 15

#define Z_AXEL_MOTOR_MODE_3 1
#define Z_AXEL_MOTOR_MODE_2 2
#define Z_AXEL_MOTOR_MODE_1 3


#define HOMING_FAILURE_UNKNOWN 0
#define HOMING_FAILURE_Z_AXEL_INTERRUPTED 4
#define HOMING_FAILURE_Z_AXEL_TOOLONG 5


static float Z_AXEL_ACC = 2500.0f;


volatile long z_axelSteps = 0;
long prev_ZAXEL_Steps = 0;
long z_axelStepsTaken = 0;

long z_axelSpeed = 0;

volatile unsigned short zaxelCompare = 0;


long z_axelPos = 0;
long z_axelAccEnd = 0;
long z_axelDeAccStart = 0;

int z_axelDir = 1;

bool z_axelHomed = false;

*/

/*// Define a circular buffer
const int bufferSize = 64;
char buffer[bufferSize];
int bufferIndex = 0;*/


#define MAX_INPUT 32


// GENERAL:
// Number 200 comes from KH42-951 stepper motor, which is 200 steps rep revolution e.g. 1,8deg/step. So, 360degree / 1,8deg/step = 200
// STS820 stepper motor driver can drive with microsteps. When used, the 1,8step/degree is divided with the used mode. 1/8 mode, 1/16 mode etc.
// In this application, used step mode is 1/8 equals 1600 step/revolution. 1,8deg/step / 8 = 0,225deg/step. 360deg / 0,225deg/step = 1600step


const long z_axel_StepperMotorStepsPerRevolution = 200 * 8;  // step mode 1/8th equals 1600step/rev
const float z_axel_GearRatio = (42.0f / 12.0f) / 360.0f;     // Z-AXEL gear ratio (42/12)/360deg=0.0097222

const long StepperMotorStepsPerRevolution = 200 * 8;  // step mode 1/8th equals 1600step/rev

//const long StepperMotorStepsPerRevolution = 200 * 8; // step mode 1/8th equals 1600step/rev
const float z_axel_StepsPerDegree = z_axel_StepperMotorStepsPerRevolution * z_axel_GearRatio;  // NOTICE! Z-AXEL stepper motor is connected to build plate stepper motor, connector J25


const long z_axel_MaxTravel = (long)(40.0 * z_axel_StepsPerDegree);  // protojig max value for tilt angle, e.g. 20 deg back and forth


const float Z_AXEL_MIN_SPEED_MM_PER_S = 0.5f;
const float Z_AXEL_MIN_SPEED_STEPS_PER_S = z_axel_StepsPerDegree * Z_AXEL_MIN_SPEED_MM_PER_S;


char z_axelToggle = 0;

unsigned short heartBeat = 0;


void setup() {
  Serial.begin(57600);
  Serial.println("");
  Serial.print("LCB2-OLED v");
  Serial.print(VERSION_MAJOR);
  Serial.print(".");
  Serial.print(VERSION_MINOR);
  Serial.println("E");
  Serial.println("Setup completed, type Z i or D s");

	lcb_init();
	delay(1500);

	setup_completed();
	delay(1500);

	ip_address_text();
	delay(1500);

	ip_address_numbers();
	delay(1500);

}

/*

void init_stepper_motors() {
  Serial.begin(57600);
   
  Serial.println("");
  Serial.println("LCB2 init done");
  Serial.println("");

  pinMode(HEARTBEAT_LED, OUTPUT);
  pinMode(MOTOR_RESET_PIN, OUTPUT);
  digitalWrite(MOTOR_RESET_PIN, HIGH);

  // stepper motor init

  cli();

  DDRB &= ~(1<<7); // Set PORTB7 as input

  sei();

  // -- Z_AXEL

  pinMode(Z_AXEL_ENABLE_PIN, OUTPUT);
  pinMode(Z_AXEL_MOTOR_MODE_3, OUTPUT);
  pinMode(Z_AXEL_MOTOR_MODE_2, OUTPUT);
  pinMode(Z_AXEL_MOTOR_MODE_1, OUTPUT);
  pinMode(Z_AXEL_STEP_PIN, OUTPUT);
  pinMode(Z_AXEL_LIMIT_PIN_UPPER, INPUT);
  pinMode(Z_AXEL_LIMIT_PIN_LOWER, INPUT);

//  setZ_AXELMotorMode(HIGH, LOW, LOW); 16th step
  setZ_AXELMotorMode(LOW, HIGH, HIGH);

  setZ_AXELDirection(DIR_STOP);

  digitalWrite(Z_AXEL_ENABLE_PIN, HIGH);

  digitalWrite(Z_AXEL_STEP_PIN, HIGH); // Known bug in 328bp

  prepareZ_AXELMotorOscillator();

  // Set build plate energy save port to output

  DDRB |= (1 << 6);
  setZ_AXELEnergySave(false);

  setZ_AXELAcceleration(1);
  setZ_AXELMaxSpeed(0.5);  

}

*/

int FreeRam() {
  extern int __heap_start, *__brkval;
  int v;
  return (int)&v - (__brkval == 0 ? (int)&__heap_start : (int)__brkval);
}


//--------Text print to OLED ------------


void lcb_init() {

  //328pb U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ 28, /* data=*/ 27);

	U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // initialization for the used OLED display

  delay(100);

  u8g2.begin();
  u8g2.clearBuffer();

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 10, "LCB initializing...");

  u8g2.sendBuffer();
}


void setup_completed() {

  //328pb U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ 28, /* data=*/ 27);

	U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // initialization for the used OLED display


  delay(100);

  u8g2.begin();
  //u8g2.clearBuffer();

	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 10, "LCB initializing...");

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 30, "* setup completed");

  u8g2.sendBuffer();
}


void ip_address_text() {

  //328pb U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ 28, /* data=*/ 27);

	U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // initialization for the used OLED display

  delay(100);

  u8g2.begin();
  //u8g2.clearBuffer();

	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 10, "LCB initializing...");

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 30, "* setup completed");

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(6, 45, "* IP address:");

  u8g2.sendBuffer();
}


void ip_address_numbers() {

  //328pb U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE, /* clock=*/ 28, /* data=*/ 27);

	U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE); // initialization for the used OLED display

  delay(100);

  u8g2.begin();
  //u8g2.clearBuffer();

	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 10, "LCB initializing...");

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 30, "* setup completed");

  //u8g2.setFont(u8g2_font_ncenB10_tr);
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(6, 45, "* IP address:");
	u8g2.setFont(u8g2_font_6x10_tr);
	u8g2.drawStr(5, 60, "* 10.10.193.135");

  u8g2.sendBuffer();
}

/*u8g2.setBitmapMode(1);
u8g2.setFontMode(1);
u8g2.setFont(u8g2_font_6x10_tr);
u8g2.drawStr(5, 10, "LCB initializing...");
u8g2.setFont(u8g2_font_6x10_tr);
u8g2.drawStr(5, 30, "* setup completed");
u8g2.setFont(u8g2_font_6x10_tr);
u8g2.drawStr(6, 45, "* IP address:");
u8g2.setFont(u8g2_font_6x10_tr);
u8g2.drawStr(5, 60, "* 10.10.193.135");*/

//---------------------------------------

/*

void setZ_AXELEnergySave(bool state)
{

  if (state) {
    PORTB |= 1 << 6;
  } else {
    PORTB &= ~(1 << 6);
  }
}


void homingFailure(unsigned int reason)
{
  Serial.print("ERROR:");
  Serial.println(reason);
  
  switch(reason)
  {
    case HOMING_FAILURE_Z_AXEL_INTERRUPTED:
      z_axelHomed = false;
    break;

    case HOMING_FAILURE_UNKNOWN:
    case HOMING_FAILURE_Z_AXEL_TOOLONG:
    default:
      //digitalWrite(NOT_IN_USE_MOTORENABLE_PIN, LOW);
      digitalWrite(Z_AXEL_ENABLE_PIN, LOW);
      while (true);
    break;
 }
}


void setZ_AXELMotorMode(unsigned char m3, unsigned char m2, unsigned char m1)
{
  cli();

  // Set Z_AXEL DIR as output
  DDRE |= 1 << 0;
  // Set Z_AXEL DIR
  PORTE |= 1 << 0;

  unsigned int pe = PORTE & (~(0b1110));
  pe |= m3 << Z_AXEL_MOTOR_MODE_3;
  pe |= m2 << Z_AXEL_MOTOR_MODE_2;
  pe |= m1 << Z_AXEL_MOTOR_MODE_1;
  PORTE = pe;

  sei();
}

void setZ_AXELDirection(int dir)
{
  z_axelDir = dir;

  //  cli();
  if (z_axelDir < 0) {
    PORTE |= 1 << 0;
  } else
  {
    PORTE &= ~1;
  }
  //  sei();
}

void prepareZ_AXELMotorOscillator()
{
  cli();//stop interrupts
  TCCR3A = 0;
  TCCR3B = 0;
  TCNT3 = 0;
  OCR3A = zaxelCompare;
  TCCR3A |= (1 << COM3B0);   // Toggle OC3A/OC3B on Compare Match.
  TCCR3B |= (1 << WGM32);    // CTC mode
  TIMSK3 |= (1 << OCIE3B);
  sei();
}

// Build plate oscillator interrupt
ISR(TIMER3_COMPB_vect)
{
  z_axelToggle = ~z_axelToggle;
  if (z_axelToggle)
  {
    --z_axelSteps;
    if (z_axelSteps <= 0) {
      TCCR3B &= ~(1 << CS30);  // clock off if done with all the steps
      z_axelSteps = 0;
    } else {
        OCR3A = zaxelCompare;
    }
  }
}

void stopZ_AXEL() {
  long stepDelta = prev_ZAXEL_Steps - z_axelSteps;
  z_axelSteps = min(z_axelSteps, Z_AXEL_MAX_SPEED);
  prev_ZAXEL_Steps = z_axelSteps + stepDelta;
}


void setZ_AXELAcceleration(float accelerationInStepsPerMM)
{
  Z_AXEL_ACC = accelerationInStepsPerMM*z_axel_StepsPerDegree;
  #ifdef DEBUG
    Serial.print("Z_AXEL_ACC: ");
    Serial.println(Z_AXEL_ACC);
  #endif
}

void setZ_AXELMaxSpeed(float maxSpeedInMMPerS)
{
  maxSpeedInMMPerS -= Z_AXEL_MIN_SPEED_MM_PER_S;
  Z_AXEL_MAX_SPEED = maxSpeedInMMPerS * z_axel_StepsPerDegree;

#ifdef DEBUG
  Serial.print("Z_AXEL MAX SPEED: ");
  Serial.println(Z_AXEL_MAX_SPEED);
#endif

}

void moveZ_AXELAbsolute(long pos)
{
  if (!z_axelHomed) {
    Serial.println("NOT HOMED!");
    return;
  }

  if (pos < 0) {
    pos = 0;
  }
  if (pos > z_axel_MaxTravel) {
    Serial.print("WARNING! Too big value.");
    Serial.println(pos);
    pos = z_axel_MaxTravel; // Adding z_axel_MaxTravel to pos variable

  }

  z_axelSteps = pos - z_axelPos;

    Serial.print("Z-AXEL steps requested: ");
    Serial.println(z_axelSteps);

  if (z_axelSteps == 0)
  {
    reportBuildPlatformAtTarget();
    return;
  }

  if (z_axelSteps > 0) {
    setZ_AXELDirection(DIR_UP);
  } else {
    z_axelSteps *= -1;
    setZ_AXELDirection(DIR_DOWN);
  }

  z_axelStepsTaken = 0;

  float posAtMaxSpeed = 0.5f * Z_AXEL_MAX_SPEED * Z_AXEL_MAX_SPEED / Z_AXEL_ACC;

  z_axelAccEnd = (long)(posAtMaxSpeed);
  if (z_axelAccEnd > z_axelSteps / 2) {
    z_axelAccEnd = z_axelSteps / 2;
    z_axelDeAccStart = z_axelSteps / 2;
  } else {
    z_axelDeAccStart = z_axelSteps - z_axelAccEnd;
  }

  bool running = TCCR3B & (1 << CS30);
  prev_ZAXEL_Steps = z_axelSteps;


  if (!running)
  {
    z_axelSpeed = Z_AXEL_MIN_SPEED_STEPS_PER_S;
    zaxelCompare = OSCILLATOR_FREQ / Z_AXEL_MIN_SPEED_STEPS_PER_S;
    OCR3A = zaxelCompare;
    TCCR3B |= (1 << CS30);     // clock on, no pre-scaler
   
  }

}


void moveZ_AXELAbsoluteMM(float mm)
{
  moveZ_AXELAbsolute((long)(mm * z_axel_StepsPerDegree + 0.5));
}

void openZ_AXEL() {
  moveZ_AXELAbsolute(z_axel_MaxTravel);
}
void closeZ_AXEL() {
  moveZ_AXELAbsolute(0);
}

void homeZ_AXEL(bool abortPossible)
{
  // Clear serial buffer
  while (abortPossible && Serial.available () > 0)
  {
    Serial.read();
  }

  bool interrupted = false;
  // If we need to move home first
  if ((PINC & 2) == 0) {

#ifdef DEBUG
    Serial.println("Homing Z-AXEL down");
#endif
    setZ_AXELDirection(DIR_DOWN); // check that Solanna Calibration Device monitor frame moves right direction after homing

    z_axelSteps = 1000000; // max steps to take
    z_axelSpeed = 0;
    zaxelCompare = HOMING_COMPARE_OFFSET_SLOW; // Z-AXEL homing speed, check new value for the parameter from defines
    OCR3A = zaxelCompare;
    TCCR3B |= (1 << CS30);     // clock on, no pre-scaler



    while ((PINC & 2) == 0 && z_axelSteps > 0 && !interrupted) {
      // Check for outside abort
      if (abortPossible && Serial.available () > 0)
      {
        homingFailure(HOMING_FAILURE_Z_AXEL_INTERRUPTED);
        interrupted = true;
        Serial.println("Homing Z-AXEL interrupted.");

      } else {
        delayMicroseconds(1);
      }

    }
    if (z_axelSteps <= 0) {
      homingFailure(HOMING_FAILURE_Z_AXEL_TOOLONG);
    }

    z_axelSteps = 0;
    TCCR3B &= ~(1 << CS30);     // clock off, no pre-scaler

  }

  if (!interrupted)
  {
    delay(10);

    const long Z_AXEL_HOMING_OFFSET = 0; // Assume home is this many steps away

    z_axelPos = Z_AXEL_HOMING_OFFSET;

    z_axelSteps = 0;
    prev_ZAXEL_Steps = 0;

    z_axelHomed = true;

    Serial.println("HOMING OK");
    Serial.print("POS: ");
    Serial.print(z_axelPos);


  }

}


void homeZ_AXELUp(boolean abortPossible)
{
    // Clear serial buffer
  while (abortPossible && Serial.available () > 0)
  {
    Serial.read();
  }

  // If we need to move home first
  if ((PINC & 1) == 0) {

#ifdef DEBUG
    Serial.println("Homing Z-AXEL");
#endif
    setZ_AXELDirection(DIR_UP);

    z_axelSteps = 1000000; // max steps to take
    z_axelSpeed = 0;
    zaxelCompare = HOMING_COMPARE_OFFSET_SLOW; // Slowest known speed
    OCR3A = zaxelCompare;
    TCCR3B |= (1 << CS30);     // clock on, no pre-scaler



    while ((PINC & 1) == 0 && z_axelSteps > 0) {
      // Check for outside abort
      if (abortPossible && Serial.available () > 0)
      {
        homingFailure(HOMING_FAILURE_Z_AXEL_INTERRUPTED);
      } else {
        delayMicroseconds(1);
      }

    }
    if (z_axelSteps <= 0) {
      homingFailure(HOMING_FAILURE_Z_AXEL_TOOLONG);
    }

    z_axelSteps = 0;
    TCCR3B &= ~(1 << CS30);     // clock off, no pre-scaler

  }

  delay(10);

  // If we need to move away from home
  if ((PINC & 1)) {

#ifdef DEBUG
    Serial.println("Homing to lower opto");
#endif
    setZ_AXELDirection(DIR_DOWN);

    z_axelSteps = 1000; // max steps to take
    z_axelSpeed = 0;
    zaxelCompare = HOMING_COMPARE_OFFSET_SLOW * 3; // Slowest known speed
    OCR3A = zaxelCompare;
    TCCR3B |= (1 << CS30);     // clock on, no pre-scaler



    while ((PINC & 1) && z_axelSteps > 0) {
      // Check for outside abort
      if (abortPossible && Serial.available () > 0)
      {
        homingFailure(HOMING_FAILURE_Z_AXEL_INTERRUPTED);
      } else {
        delayMicroseconds(1);
      }
    }
    if (z_axelSteps <= 0) {
      homingFailure(HOMING_FAILURE_Z_AXEL_TOOLONG);
    }
    z_axelSteps = 0;
    TCCR3B &= ~(1 << CS30);     // clock off, no pre-scaler

  }
  delay(5);

  z_axelPos = z_axel_MaxTravel;
  prev_ZAXEL_Steps = 0;
  z_axelSteps = 0;

  z_axelHomed = true;

  Serial.println("HOMING OK");
  Serial.println("POS: ");
  Serial.println(z_axelPos);


}

void enableZ_AXEL()
{
  digitalWrite(Z_AXEL_ENABLE_PIN, HIGH);
}
void disableZ_AXEL()
{
  digitalWrite(Z_AXEL_ENABLE_PIN, LOW);
}

void processCommand(const char *command, unsigned int len)
{
  Serial.print ("CMD: ");
  Serial.println (command);

  if (len < 3) {
    Serial.print("TYPE AGAIN");
    Serial.println (command);
    return;
  }

  if (command[0] == 'Z') // LCB NRO pin J25
  {
    switch (command[2])
    {




      case 'd' :
        disableZ_AXEL(); break;
      case 'E' :
        enableZ_AXEL(); break;
      case 'P' :
        {
          long steps = strtol (command + 3, NULL, 10);
          moveZ_AXELAbsolute(steps);           
          break;
        }
      case 'V' :
        {
          float v = atof(command + 3);
          setZ_AXELMaxSpeed(v);
          break;
        }
      case 'r' : ; // r like RAM
        {
          Serial.println("RAM");
          Serial.println(FreeRam());

          break;
        }
      case 'i' : ; // i like init stepper motor
        {
          Serial.println("Init stepper motor...");
          init_stepper_motors();
          break;
        }

      case 'h':
        homeZ_AXEL(true); break; // moving to "home" opto limit
      case 'u': // homing to "upper" opto limit
        homeZ_AXELUp(true); break;
      case 'F':
        setZ_AXELEnergySave(false); break;
      case 'f':
        setZ_AXELEnergySave(true); break;
      case 'S' :
        stopZ_AXEL(); break;
      default:
        break;
    }
  } else if (command[0] == 'D') // D like display
  {
    switch (command[2])
    {
      case 'i' : ; // i like initialize OLED display
        {
          initOLED();
          break;
        }

      case 's' : ; // s like initialize OLED display
        {
          StartinitOLED();
          break;
        }

      default:
        break;
    }

  } else if (command[0] == 'R')
  {
    switch (command[2])
    {
      case 'A' : ; //Reset Atmel
        {
          Serial.println("");
          Serial.println("RESET!");

          break;
        }

      default:
        break;
    }
  }
}


void processIncomingByte (const byte b) 
{
  static char input_line [MAX_INPUT];
  static unsigned int input_pos = 0;

  switch (b) 
  {

    case '\n':   // end of text
      input_line [input_pos] = 0;  // terminating null byte

      // terminator reached! process input_line here ...
      processCommand(input_line, input_pos);

      // reset buffer for next time
      input_pos = 0;
      break;

    case '\r':   // discard carriage return
      break;

    default:
      // keep adding if not full ... allow for terminating null byte
      if (input_pos < (MAX_INPUT - 1))
        input_line [input_pos++] = b;
      break;

  }  // end of switch

} // end of processIncomingByte


bool buildPlatformAtLimit()
{
  return digitalRead(Z_AXEL_LIMIT_PIN_UPPER) == LOW || digitalRead(Z_AXEL_LIMIT_PIN_LOWER) == LOW;

}


void reportBuildPlatformAtTarget()
{
  Serial.println("");
  Serial.print("Z-AXEL POS: ");
  Serial.print(z_axelPos);
  Serial.println("");
  z_axelDir = 0;
}


long calculateZ_AXELSpeed(long stepsTaken, long stepsRemaining)
{
  long retval;
  if (stepsTaken <= z_axelAccEnd){
    retval = sqrt(2.0f * stepsTaken * Z_AXEL_ACC);
  } else
  if (stepsRemaining <= z_axelAccEnd)
  {
    retval = sqrt(2.0f * stepsRemaining * Z_AXEL_ACC);
  } else {
    retval = Z_AXEL_MAX_SPEED;
  }
  retval += Z_AXEL_MIN_SPEED_STEPS_PER_S;
  return retval;
}

*/

//static bool hbOn = true; //status led not in use


//----------------------------------------------------------------
//OLED and Z-AXEL STEPPER MOTOR LOOP
//----------------------------------------------------------------


void loop() {

	Serial.println("LCB running...");

	delay(1500);


/*
  /*#if 1
  // Heartbeat LED
  ++heartBeat;
  if (heartBeat == 0)
  {
    hbOn = !hbOn;
    if (hbOn) {
      PORTB &= ~(1 << 5);
    } else {
      PORTB |= 1 << 5;
    }
  }
  #endif

  // If serial data available, process it
  if (Serial.available () > 0)
  {
    processIncomingByte (Serial.read ());
  }

  if (z_axelSpeed)

  {
    cli(); // Stop interrupts
    long z_axelStepsTemp = z_axelSteps;
    sei();

    long z_axelPosDelta = prev_ZAXEL_Steps - z_axelStepsTemp;
    bool last_ZAXELMove = (z_axelStepsTemp == 0 && prev_ZAXEL_Steps > 0);
    bool Z_AXELAtLowerLimit = PINC & 2;
    bool Z_AXELAtUpperLimit = PINC & 1;

    if ((z_axelDir > 0 && Z_AXELAtUpperLimit)) {
      z_axelPos += z_axelPosDelta;
      z_axelSteps = 0;
      prev_ZAXEL_Steps = 0;
      z_axelStepsTaken = 0;
      z_axelSpeed = 0;
      zaxelCompare = (unsigned short)(OSCILLATOR_FREQ / Z_AXEL_MIN_SPEED_STEPS_PER_S + 0.5f);
      reportBuildPlatformAtTarget();
    } else if (z_axelPosDelta != 0) {
      z_axelPos += z_axelPosDelta * z_axelDir;
      prev_ZAXEL_Steps = z_axelStepsTemp;
      z_axelStepsTaken += z_axelPosDelta;

      z_axelSpeed = calculateZ_AXELSpeed(z_axelStepsTaken, z_axelStepsTemp);

      unsigned short zaxelCompareTemp = OSCILLATOR_FREQ / z_axelSpeed;
      cli();
      zaxelCompare = zaxelCompareTemp;
      sei();

    // Print a "." character after each step
    Serial.print(".");

    // Check if 50 steps have been reached and add a line feed
    static int stepCounter = 0;
    stepCounter++;
    if (stepCounter >= 50) {
      Serial.println();
      stepCounter = 0; // Reset the step counter
      }

      if (last_ZAXELMove) {
        z_axelSpeed = 0;
        reportBuildPlatformAtTarget();
      }
      

    }
  }
  // -- Z AXEL steppercontrol ends

*/

}  



//processIncomingByte using while...didn't work

// Define a flag to control motor movement
/*bool moveMotorToPos100 = false;

void loop() {
  // Check if a command to move to position 100 is received
  if (moveMotorToPos100) {
    // Move the motor to position 100
    // Implement the logic to move the motor to position 100
    // ...

    // After reaching position 100, reset the flag
    moveMotorToPos100 = false;
  }

  // If serial data available, process it
  while (Serial.available() > 0) {
    char incomingByte = Serial.read();
    processIncomingByte(incomingByte);
  }

  // If z_axelSpeed is non-zero, spin the stepper motor
  if (z_axelSpeed) {
    long z_axelStepsTemp;
    cli(); // Stop interrupts
    z_axelStepsTemp = z_axelSteps;
    sei();

    long z_axelPosDelta = prev_ZAXEL_Steps - z_axelStepsTemp;

    if (z_axelPosDelta != 0) {
      z_axelPos += z_axelPosDelta * z_axelDir;
      prev_ZAXEL_Steps = z_axelStepsTemp;
      z_axelStepsTaken += z_axelPosDelta;

      z_axelSpeed = calculateZ_AXELSpeed(z_axelStepsTaken, z_axelStepsTemp);

      unsigned short zaxelCompareTemp = OSCILLATOR_FREQ / z_axelSpeed;
      cli();
      zaxelCompare = zaxelCompareTemp;
      sei();

      if (z_axelPosDelta > 0 && (PINC & 1)) {
        // Reached upper limit, stop motor
        z_axelSpeed = 0;
        reportBuildPlatformAtTarget();
      }
    }
  }
}*/




//----------------------------------------------------------------


//Original processIncomingByte loop...didn't work

/*void loop() {

  // If serial data available, process it
  if (Serial.available () > 0)
  {
    processIncomingByte (Serial.read ());
  }

  if (z_axelSpeed)

  {
    cli(); // Stop interrupts
    long z_axelStepsTemp = z_axelSteps;
    sei();

    long z_axelPosDelta = prev_ZAXEL_Steps - z_axelStepsTemp;
    bool last_ZAXELMove = (z_axelStepsTemp == 0 && prev_ZAXEL_Steps > 0);
    bool Z_AXELAtLowerLimit = PINC & 2;
    bool Z_AXELAtUpperLimit = PINC & 1;

    if ((z_axelDir > 0 && Z_AXELAtUpperLimit)) {
      z_axelPos += z_axelPosDelta;
      z_axelSteps = 0;
      prev_ZAXEL_Steps = 0;
      z_axelStepsTaken = 0;
      z_axelSpeed = 0;
      zaxelCompare = (unsigned short)(OSCILLATOR_FREQ / Z_AXEL_MIN_SPEED_STEPS_PER_S + 0.5f);
      reportBuildPlatformAtTarget();
    } else if (z_axelPosDelta != 0) {
      z_axelPos += z_axelPosDelta * z_axelDir;
      prev_ZAXEL_Steps = z_axelStepsTemp;
      z_axelStepsTaken += z_axelPosDelta;

      z_axelSpeed = calculateZ_AXELSpeed(z_axelStepsTaken, z_axelStepsTemp);

      unsigned short zaxelCompareTemp = OSCILLATOR_FREQ / z_axelSpeed;
      cli();
      zaxelCompare = zaxelCompareTemp;
      sei();

    // Print a "." character after each step
    Serial.print(".");

    // Check if 50 steps have been reached and add a line feed
    static int stepCounter = 0;
    stepCounter++;
    if (stepCounter >= 50) {
      Serial.println();
      stepCounter = 0; // Reset the step counter
      }

      if (last_ZAXELMove) {
        z_axelSpeed = 0;
        reportBuildPlatformAtTarget();
      }
      

    }
  }
  // -- Z AXEL steppercontrol ends
}*/



//To save RAM, this is outside the loop...didn't work

/*void processSerialInput() {
  // Process the contents of the buffer
  // Implement your command parsing logic here

  // Example: Print received command
  Serial.print("Received command: ");
  for (int i = 0; i < bufferIndex; ++i) {
    Serial.print(buffer[i]);
  }
  Serial.println();

  // Reset buffer
  bufferIndex = 0;
}

/*void loop() {
  // If serial data available, store it in the buffer
  while (Serial.available() > 0) {
    char incomingByte = Serial.read();

    // Store in the buffer
    buffer[bufferIndex] = incomingByte;
    bufferIndex = (bufferIndex + 1) % bufferSize;

    // Check for a complete command or limit the buffer size
    if (incomingByte == '\n' || bufferIndex == bufferSize - 1) {
      processSerialInput();
    }
  }

  // If z_axelSpeed is non-zero, spin the stepper motor
  if (z_axelSpeed) {
    long z_axelStepsTemp;

    // Only disable interrupts around critical section
    cli(); // Stop interrupts
    z_axelStepsTemp = z_axelSteps;
    sei(); // Enable interrupts

    long z_axelPosDelta = prev_ZAXEL_Steps - z_axelStepsTemp;

    if (z_axelPosDelta != 0) {
      // Disable interrupts only around critical section
      cli(); // Stop interrupts
      z_axelPos += z_axelPosDelta * z_axelDir;
      prev_ZAXEL_Steps = z_axelStepsTemp;
      z_axelStepsTaken += z_axelPosDelta;

      z_axelSpeed = calculateZ_AXELSpeed(z_axelStepsTaken, z_axelStepsTemp);

      unsigned short zaxelCompareTemp = OSCILLATOR_FREQ / z_axelSpeed;
      zaxelCompare = zaxelCompareTemp;
      sei(); // Enable interrupts

      if (z_axelPosDelta > 0 && (PINC & 1)) {
        // Reached upper limit, stop motor
        z_axelSpeed = 0;
        reportBuildPlatformAtTarget();
      }
    }
  }
}*/