#include <avr/interrupt.h>
#include <math.h>
#include <stdint.h>
//#include <EEPROM.h>
#include "Arduino.h"
#include "curveFitting.h"
#include <FastLED.h>
#include "PinChangeInterrupt.h"
//#include <PlotPlus.h>

#define NUM_LEDS 60 // Number of LEDs in the strip
#define DATA_PIN 8 // Data pin for the LED strip
#define WAVE_WIDTH 25 // Width of each wave (number of LEDs lit at once)
#define NUM_WAVES 2 // Number of waves active at once
int lastLedStripTime = 0;
#define LED_STRIP_FREQUENCY_CHECK 50 // millisecond
CRGB leds[NUM_LEDS]; // Array to hold the LED colors
CRGB ledStripColor = CRGB::Lime;
unsigned long previousLedStripTime = 0; // Stores the last time the wave was updated
int ledSpeed = 20; // Speed of the wave (higher value means faster)
//int direction = 1; // Direction of the wave (1 for forward, -1 for backward). Commenting code to use the directionUp variable
uint8_t maxBrightness = 255; // Maximum brightness at the center of the wave
uint8_t minBrightness = 0; // Minimum brightness at the edges of the wave

/*
 Definitions for the encoder
 Calculation of distance per revolution:
 The drive pulley has 26 teeth and the T5 profile has a pitch of 5 mm.
 Therefore, the distance per revolution is 26 * 5 mm = 130 mm.
 */
volatile long encoderPosition = 0; // used on interrupt PIN
long lastEncoderPosition = encoderPosition;
volatile bool directionUp = false; // used on interrupt PIN
//volatile unsigned long pulseWidth = 0;
#define DEBOUNCE_ENCODER_THRESHOLD 200 // Debounce threshold in microseconds
// volatile unsigned long lastPulseTime = 0;
float speed = 0.0;
float positionMm = 0.0;
float startPositionMm = 1885.0;
#define FIRST_START_POSITION_MM 1885.0
#define LED_SPEED_FACTOR 10000
#define MIN_ENC_COUNTER 1 // minimal encoder counting difference to detect transition from stages
#define START_POSITION_ADDRESS 201 // EEPROM address for saving startPositionMM
#define SIGN_A_PIN 2 // Encoder Sign A used on interrupt PIN
#define SIGN_B_PIN 3 // Encoder Sign B used on interrupt PIN

// Variables for distance calculation
#define DRIVE_PULLEY_TEETH 26 // Number of teeth on the drive pulley
#define PROFILE_PITCH 5.0 // Pitch of the profile in mm
#define PULSES_PER_REVOLUTION 32 // Pulses per revolution of the encoder
// Define the sampling period (in milliseconds)
#define SAMPLING_SPEED_PERIOD 200.0 // Adjust this value as needed
// Define variables for EMA smoothing.
//#define EMA_FACTOR_MS 2000 // in milliseconds
#define EMA_SPD_FACTOR_NORMAL 0.1 // 1 - exp(-((float)SAMPLING_SPEED_PERIOD / EMA_FACTOR_MS))
#define EMA_SPD_FACTOR_NEAR_ZERO 0.8 // 1 - exp(-((float)SAMPLING_SPEED_PERIOD / (EMA_FACTOR_MS / EMA_FAST_FACTOR_MULT))) // higher factor when the value is close to 0
#define LOW_SPEED_THRESHOLD 50.0 // m/s
#define LOW_SPEED_NEAR_ZERO 5 // m/s
// const float TIME_CONSTANT = 2000.0; // Adjust this value as needed
// Calculate the smoothing factor (alpha)
// const float SMOOTH_FACTOR = SAMPLING_SPEED_PERIOD / (TIME_CONSTANT + SAMPLING_SPEED_PERIOD); // Smoothing factor for slowing values change: weight, speed, acceleration
// Variables to store the raw and filtered values
//int rawValue = 0; // speed
//float filteredValue = 0.0; // smoothedSpeed
// Calculate the distance per revolution
//#define ENCODER_TPX_FACTOR_RELATION 2.0
//#define ENCODER_DPX_FACTOR_RELATION 1.0
#define ENCODER_FACTOR_RELATION 2.0
#define DISTANCE_PER_REVOLUTION (DRIVE_PULLEY_TEETH * PROFILE_PITCH) // Distance per revolution in mm
#define DISTANCE_PER_PULSE (DISTANCE_PER_REVOLUTION / PULSES_PER_REVOLUTION) // Distance per encoder pulse in mm
// const unsigned long speedSmoothTime = 200; // Time to smooth speed updates (in milliseconds)
// volatile float lastSpeed = 0.0; // Store the previous speed to calculate acceleration
volatile float acceleration = 0.0; // Acceleration in mm/s²
#define SHOW_PERIOD 300 // Time interval to show speed and position
unsigned long lastShowTime = 0; // Last time speed and position were shown
// unsigned long lastSpeedSmoothTime = 0; // Last time speed was smoothed
float smoothedSpeed = 0.0; // Smoothed speed to display
float startSpeedPosition = 0.0;
float lastForkPosition = 0.0;
float lastSpeedTime = 0.0;
bool startPositionSet = false; // Flag to indicate if the start position has been set

// ProximitySensor input
#define PROXIMITY_SENSOR 7 // Proximity switch pin input
#define PROX_SENSOR_LVL_LOGIC HIGH // Change to LOW if the sensor is normally low
volatile bool lastProxSensState = !PROX_SENSOR_LVL_LOGIC;
volatile bool proxSensCurrentLvl = !PROX_SENSOR_LVL_LOGIC;
#define PROX_SENSOR_VLD_WDTH 70 // Width in milliseconds
volatile unsigned long lastProxSensorHighTime = 0;
#define ANALOG_READ_FREQUENCY 150 // read analog input every 200ms
int readingCount = 0;
#define MAX_NUMBER_RD_COUNT 3
// int order; // Global variable to store the order of the polynomial
#define MAX_POLYNOMIAL_ORDER 3 // change this to the maximal polynomial order in the software
double* coeffs; // Global pointer to store coefficients. Coeffs must be calculated using fit curve function every time before using.

// Conversion of voltage values
#define V1_START_ADDR 0 // EEPROM x1 array start address
#define V2_START_ADDR 50 // EEPROM x2 array start address
#define W_START_ADDR 100 // EEPROM y array start address
#define ARRAY_SIZE_WEIGHT_SENSOR 12 // Adjusted to match the provided arrays for weight sensor acquisition (below)
#define RESIDUAL_ARRAY_SIZE 7
// const double V1_AD[ARRAY_SIZE_WEIGHT_SENSOR] = { 150, 172, 213, 248, 250, 279, 291, 335, 352, 372, 415, 456 }; // First stage values - x coordinate
const double V1_AD[ARRAY_SIZE_WEIGHT_SENSOR] = { 115, 137, 178, 213, 215, 244, 256, 300, 317, 337, 380, 421 }; // First stage values - x coordinate
// const double V2_AD[ARRAY_SIZE_WEIGHT_SENSOR] = { 217, 238, 298, 343, 348, 383, 392, 447, 462, 486, 537, 584 }; // Second stage values - x coordinate
const double V2_AD[ARRAY_SIZE_WEIGHT_SENSOR] = { 170, 191, 251, 296, 301, 336, 345, 400, 415, 439, 490, 537 }; // Second stage values - x coordinate
const double WT_FIRST_SAMPLES[ARRAY_SIZE_WEIGHT_SENSOR] = { 100.6, 201.2, 402, 579, 602.2, 748.4, 796.8, 1005, 1105.4, 1205.8, 1408.8, 1608.2 }; // Corresponding weights - y coordinate
// double weightSampleDpx[RESIDUAL_ARRAY_SIZE] = { 750, 850, 1050, 1200, 1300, 1450, 1600 }; // Residual Duplex table weight sample values - x coordinate
double weightSampleTpx[RESIDUAL_ARRAY_SIZE] = { 575, 750, 1000, 1100, 1200, 1400, 1600 }; // Residual Triplex table weight sample values - x coordinate
// double heightSampleDpx[RESIDUAL_ARRAY_SIZE] = { 4644, 4144, 3544, 3244, 2944, 2744, 2544 }; // Duplex residual table - y coordinate
double heightSampleTpx[RESIDUAL_ARRAY_SIZE] = { 5466, 4716, 3866, 3666, 3466, 2966, 2766 }; // Triplex residual table - y coordinate
// bool tpxType = true; // Variable to check if the truck has "triplex" mast. This was define as variable so it could be changed on the fly (while the code is running).
double heightLimit;
double truckH3 = 5466; // insert truck tower height
double truckHeightLimit = 3470; // insert height limitation (ex.: max tower height or lifting limitation)
#define DIFF_GREEN_H_VALUE 1000 // value in millimeter
#define SECURITY_HEIGHT_LIMIT 300 // value in millimeter
#define HEIGHT_HYSTERESIS 500
double* v1AdjustedAD;
double* v2AdjustedAD;
/*
#define SAMPLE_WEIGHT_1ST_STAGE 1392.0/1200.0
#define SAMPLE_WEIGHT_2ND_STAGE 1392.0/1200.0
*/
/*
#define COMP_ARRAY_SIZE 3
double weightADMeasured1stStage[COMP_ARRAY_SIZE] = { -167, 700, 1200 }; // x coord
double weightADMeasured2ndStage[COMP_ARRAY_SIZE] = { -137.26, 800, 1200 }; // x coord
double realWeight[COMP_ARRAY_SIZE] = { 0, 1042, 1392 }; // y coord
*/
/*
double xRef[] = {98, 372};
double yRef[] = {0, 1392};
*/
#define TOLERANCE 0.0001 // used on findX
#define MAX_ITERATIONS 100 // used on findX
#define SPD_FACTOR_POS -1.6
#define SPD_FACTOR_NEG -0.2
#define ACCEL_FACTOR_POS -0.5
#define ACCEL_FACTOR_NEG -0.25
int weightSensorValue = 0; // used on interrupt ISR()
unsigned long lastWeightSmoothTime = 0; // Last time weight was smoothed
float smoothedWeight = 0.0; // Smoothed weight after EMA smooth function
int roundedWeight50Kg; // Weight rounded value each 50kg
// #define FIRST_STEP_ROUND 70 // used to round a first step before round each 50 kg
float currentWeight = 0;
// float lastWeight = 0;
#define DIFF_WEIGH_FAST_SMOOTH 200 // weight difference to fastening smooth update
#define DIFF_WEIGH_FORCE_SMOOTH 200 // weight difference to force smooth update
#define SAMPLING_WEIGHT_PERIOD 400.0 // Adjust this value as needed
#define EMA_WT_FACTOR_NORMAL 0.08 // 1 - exp(-((float)SAMPLING_WEIGHT_PERIOD / EMA_FACTOR_MS))
#define EMA_WT_FACTOR_NEAR_ZERO 0.2 // 1 - exp(-((float)SAMPLING_WEIGHT_PERIOD / (EMA_FACTOR_MS / EMA_FAST_FACTOR_MULT))) // higher factor when the value is close to 0
#define EMA_SENS_WT_FACTOR 0.1 // 1 - exp(-((float)SAMPLING_WEIGHT_PERIOD / (EMA_FACTOR_MS / EMA_FAST_FACTOR_MULT))) // higher factor when the value is close to 0
#define LOW_WEIGHT_NEAR_ZERO 20
// #define LOW_WEIGHT_THRESHOLD 200
#define IGNORE_AFTER_START_2ND_STAGE 2000.0 // Value in millisecond to ignore after measure in 2nd stage
bool measureHeightValid = false;
#define WEIGHT_SENSOR_PIN_INPUT A0 // used on interrupt ISR()
#define LIFTING_OUTPUT 12 // Output pin to enable or disable lifting
bool liftLogicalOutputEnableHigh = true; // Considering true when Output is high to enable and low to disable. False for the opposite
// volatile unsigned long currentMillis1 = 0;
// volatile unsigned long currentMillis2 = 0;
// volatile unsigned long currentMillis3 = 0;
// volatile unsigned long timerMillis1 = 0;
// volatile unsigned long timerMillis2 = 0;
// volatile unsigned long timerMillis3 = 0;
volatile unsigned long lastTimeAnalogRead = 0;
volatile unsigned long lastBlink = 0;
// volatile unsigned long previousMillis3 = 0;
/*
int plotBuffer;
char buffer;
 */
#define OFFSET 500
// Function bits to use as internal states. Limit to 8 bits. Change unint8_t to uint16_t and so on, if necessary more bits.
volatile uint8_t functionBits = 0;
#define ANALOG_READ 0
#define STAGE_TEMP_BIT 1
#define STAGE_VALID_BIT 2
#define PROX_BIT_ISR 3
#define PROX_BIT_STATE 4
// Select the timers you're using, here ITimer1
#define TIMER_INTERVAL_100MS 100
#define TIMER_INTERVAL_1000MS 1000
#define TIMER_INTERVAL_2000MS 2000
#define TIMER_INTERVAL_1MS 1
#define LED_ON_TIME 100
#define LED_OFF_TIME1 900
#define LED_OFF_TIME2 1900
#define NUMB_READINGS1 1000
#define BUTTON1 4
#define BUTTON2 5
bool stageBitState = HIGH; // The current state of the output pin
int button1State; // The current reading from the input pin
bool lastButton1State = LOW; // The previous reading from the input pin
// The following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime1 = 0; // The last time the output pin was toggled
#define DEBOUNCE_DLY1 50 // The debounce time; increase if the output flickers

// Function to generate a triangular wave effect
void wavePattern(CRGB color, int speed, bool direction) {
	static int positions[NUM_WAVES]; // Array to hold positions of multiple waves
	static bool initialized = false;
	if (!initialized) {
		for (int i = 0; i < NUM_WAVES; i++) {
			positions[i] = i * (NUM_LEDS / NUM_WAVES); // Spread waves across the strip
		}
		initialized = true;
	}
	// Clear the LED strip
	fill_solid(leds, NUM_LEDS, CRGB::Black);
	// Iterate through each wave
	for (int w = 0; w < NUM_WAVES; w++) {
		// Create a wave for each position in positions array
		for (int i = -WAVE_WIDTH / 2; i <= WAVE_WIDTH / 2; i++) {
			int ledPos = positions[w] + i;
			// Wrap the position around if it goes out of bounds
			if (ledPos >= NUM_LEDS) ledPos -= NUM_LEDS;
			if (ledPos < 0) ledPos += NUM_LEDS;
			// Calculate brightness for a triangular wave
			uint8_t brightness = maxBrightness - abs(i) * ((maxBrightness - minBrightness) / (WAVE_WIDTH / 2 + 1));
			if (brightness < minBrightness) brightness = minBrightness;
			// Set the color with the adjusted brightness
			leds[ledPos] = color;
			leds[ledPos].fadeLightBy(256 - brightness);
		}
		// Update the wave position based on the directionUp variable
		if (direction == 1) {
			positions[w]++;
			if (positions[w] >= NUM_LEDS) positions[w] = 0;
		} else {
			positions[w]--;
			if (positions[w] < 0) positions[w] = NUM_LEDS - 1;
		}
	}
	// Show the updated LED colors
	FastLED.show();
}

void blinkLed() {
	unsigned long currentTimer = millis();
	unsigned long elapsedMillis2 = currentTimer - lastBlink;
	if (digitalRead(LED_BUILTIN) && elapsedMillis2 >= LED_ON_TIME) {
		digitalWrite(LED_BUILTIN, LOW);
		lastBlink = currentTimer;
	}
	if (!digitalRead(LED_BUILTIN) && elapsedMillis2 >= LED_OFF_TIME1 && !bitRead(functionBits, STAGE_VALID_BIT)) { // Different timers according to stage bit 0 = 1st stage 1 = 2nd stage
		digitalWrite(LED_BUILTIN, HIGH);
		lastBlink = currentTimer;
	}
	if (!digitalRead(LED_BUILTIN) && elapsedMillis2 >= LED_OFF_TIME2 && bitRead(functionBits, STAGE_VALID_BIT)) {
		digitalWrite(LED_BUILTIN, HIGH);
		lastBlink = currentTimer;
	}
}

void readAnalogInputs() { // Include all analog inputs on this function
	unsigned long currentTime = millis();
	int lastWeightSensorValue = weightSensorValue;
	if (currentTime - lastTimeAnalogRead >= ANALOG_READ_FREQUENCY) {
		int currrentWeightSensorValue = analogRead(WEIGHT_SENSOR_PIN_INPUT);
		weightSensorValue = ema(lastWeightSensorValue, currrentWeightSensorValue, EMA_SENS_WT_FACTOR);
		bitSet(functionBits, ANALOG_READ);
		// aVariableToStoreValue = analogRead(anInputPinNumber); // pattern to read an analog input using the timer interrupt
		lastTimeAnalogRead = currentTime;
	}
}

double evaluatePolynomial(double x, double *coeffs, int order) {
	double y = 0;
	for (int i = 0; i <= order; i++) {
		y += coeffs[i] * pow(x, order - i);
	}
	return y;
}

// Function to evaluate the derivative of the polynomial
double evaluatePolynomialDerivative(double x, double *coeffs, int order) {
    double y = 0;
    for (int i = 0; i < order; i++) {
        y += coeffs[i] * (order - i) * pow(x, order - i - 1);
    }
    return y;
}


// Newton-Raphson method
 double findX(double *coeffs, int order, double y, double initialGuess, double tolerance, int maxIterations) {
 	double x = initialGuess;
 	for (int i = 0; i < maxIterations; i++) {
 		double fx = evaluatePolynomial(x, coeffs, order) - y; // Adjusted to solve f(x) - y = 0
 		double fpx = evaluatePolynomialDerivative(x, coeffs, order);
 		if (fabs(fpx) < tolerance) {
 			// Avoid division by zero
 			break;
 		}
 		double x1 = x - fx / fpx;
 		if (fabs(x1 - x) < tolerance) {
 			return x1; // Converged
 		}
 		x = x1;
 	}
 	return x; // Return the last computed value if not converged
 }

 void printFormula(int ret, int order, const char* name){
 	Serial.println(name);

 	if (ret == 0) {  // Returned value is 0 if no error
 		Serial.print("f(x) = ");
 		for (int i = 0; i <= order; i++) {
 			if (i > 0 && coeffs[i] >= 0) {
 				Serial.print(" + ");
 			}
 			Serial.print(coeffs[i], 10);
 			if (i < order) {
 				Serial.print(" * x");
 				if (order - i > 1) {
 					Serial.print("^");
 					Serial.print(order - i);
 				}
 			}
 		}
 		Serial.println();
 	}

 }

 void adjustXValues(double* oldX, double* originalY, int originalSize, int originalOrder, double* xRef, double* yRef, int refSize, int referOrder, double* newX) {

 	double* diffX = new double[originalSize];
 	double* tempX = new double[originalSize];

 	// Fit the original polynomial using fitCurve
     int ret = fitCurve(originalOrder, originalSize, oldX, originalY, originalOrder + 1, coeffs);
     // Calculate diffX array
     printFormula(ret, originalOrder, "Original polynomial formula: ");

     for (int i = 0; i < originalSize; i++) {
         double findXValue = findX(coeffs, originalOrder, originalY[i], oldX[i], TOLERANCE, MAX_ITERATIONS);
         diffX[i] = oldX[i] - findXValue;
     }
  	printArray(diffX,originalSize,"diffX");


     // Fit the reference polynomial using fitCurve
     ret = fitCurve(referOrder, refSize, xRef, yRef, referOrder + 1, coeffs);
     printFormula(ret, referOrder, "Reference polynomial formula: ");


     // Calculate tempX array
     for (int i = 0; i < originalSize; i++) {
         tempX[i] = findX(coeffs, referOrder, originalY[i], oldX[i], TOLERANCE, MAX_ITERATIONS);
     }

  	printArray(tempX,originalSize,"tempX");

    // Calculate newX array and round each element
     for (int i = 0; i < originalSize; i++) {
         newX[i] = tempX[i] - diffX[i];
     }

 	printArray(newX,originalSize,"newX");

  	delete[] diffX;
 	delete[] tempX;

  	for (int i = 0; i < originalSize; i++) {
 		newX[i] = round(newX[i]);
 	}
 }


void buttonActuation() {
	// Read the state of the switch into a local variable:
	int reading = digitalRead(BUTTON1);
	unsigned long currentTime = millis();
	// Check to see if you just pressed the button
	// (i.e. the input went from LOW to HIGH), and you've waited long enough
	// since the last press to ignore any noise:
	// If the switch changed, due to noise or pressing:
	if (reading != lastButton1State) {
		// Reset the debouncing timer
		lastDebounceTime1 = currentTime;
	}
	if ((currentTime - lastDebounceTime1) > DEBOUNCE_DLY1) {
		// Whatever the reading is at, it's been there for longer than the debounce
		// delay, so take it as the actual current state:
		// If the button state has changed:
		if (reading != button1State) {
			button1State = reading;
			// Only toggle the LED if the new button state is HIGH
			if (button1State == HIGH) {
				stageBitState = !stageBitState;
			}
		}
	}
	// Set the StageBit in functionBits:
	bitWrite(functionBits, STAGE_VALID_BIT, stageBitState);
	// Save the reading. Next time through the loop, it'll be the lastButtonState:
	lastButton1State = reading;
}
/*
void saveArrayToEeprom(double* array, int startAddr, int size) {
	for (int i = 0; i < size; i++) {
		int addr = startAddr + i * sizeof(double);
		if (EEPROM.read(addr) == 0xFF | array[i] != EEPROM.read(addr)) {  // Check if EEPROM address is empty or if it has different value
			EEPROM.put(addr, array[i]);
		}
	}
}

void readArrayFromEeprom(double* array, int startAddr, int size) {
	for (int i = 0; i < size; i++) {
		int addr = startAddr + i * sizeof(double);
		EEPROM.get(addr, array[i]);
	}
}
*/
void printArray(double* array, int size, const char* name) {
	Serial.print(name);
	Serial.print(": ");
	for (int i = 0; i < size; i++) {
		Serial.print(array[i]);
		Serial.print(" ");
	}
	Serial.println();
}
/*
void addToArrayInEeprom(double value, int startAddr, int& size) {
	int addr = startAddr + size * sizeof(double);
	EEPROM.put(addr, value);
	size++;
}

void updateEepromIfValueExists(double* array, double* x1, double* x2, double* y, int startAddr, int size) {
	for (int i = 0; i < size; i++) {
		int addr = startAddr + i * sizeof(double);
		double storedValue;
		EEPROM.get(addr, storedValue);
		if (storedValue == y[i]) {
			EEPROM.put(startAddr + i * sizeof(double), array[i]);
		}
	}
}

void saveFirstStartPosition(float newStartPositionMm) {
	if (EEPROM.read(START_POSITION_ADDRESS) == 0xFF) {  // Check if EEPROM address is empty
		startPositionMm = newStartPositionMm;
		EEPROM.put(START_POSITION_ADDRESS, startPositionMm);
		Serial.print("New Start Position Saved: ");
		Serial.print(startPositionMm);
		Serial.println(" mm");
	} else {
		Serial.print("New Start Position Not Saved: ");
		Serial.print(startPositionMm);
		Serial.println(" mm");
	}
}
*/
float calculateSpeed(float deltaDistance, float timeDifference) {
	return deltaDistance / timeDifference;
}

float calculateAcceleration(float deltaSpeed, float timeDifference) {
	if (deltaSpeed != 0.0 ){
		return deltaSpeed / timeDifference;
	}
	else return 0;
}

void proximitySensorISR(){

	lastProxSensState = digitalRead(PROXIMITY_SENSOR);
	lastProxSensorHighTime = millis();
	bitSet(functionBits, PROX_BIT_ISR); // Set the bit
}
void proximitySensorHandle(){
	if (bitRead(functionBits,PROX_BIT_ISR)) {
		unsigned long currentTime = millis();
		if ((currentTime - lastProxSensorHighTime) >= PROX_SENSOR_VLD_WDTH) {
			bitClear(functionBits,PROX_BIT_STATE);
			if (digitalRead(PROXIMITY_SENSOR)==lastProxSensState){
				proxSensCurrentLvl = lastProxSensState;
				if (proxSensCurrentLvl == PROX_SENSOR_LVL_LOGIC) {
					bitSet(functionBits, PROX_BIT_STATE); // Set the bit
				} else {
					bitClear(functionBits, PROX_BIT_STATE); // Clear the bit
				}
			}
		}
	}
}

void encoderA() {
	if (digitalRead(SIGN_A_PIN) == !digitalRead(SIGN_B_PIN)) {
		encoderPosition++;
		directionUp = true;
		bitSet(functionBits, STAGE_TEMP_BIT);
	}
}

void encoderB() {
	if (digitalRead(SIGN_B_PIN) == !digitalRead(SIGN_A_PIN)) {
		encoderPosition--;
		directionUp = false;
		bitSet(functionBits, STAGE_TEMP_BIT);
	}
}


double ema(double prevVal, double currVal, double factor) {
	double emaVal = (factor * currVal) + ((1 - factor) * prevVal);
	return emaVal;
}

float getAdjustedtedWeight(float weightToCorrect, float compensationSpeed, float compensationAcceleration, int weightOrder, double* weightCoeffs) {
	float speedFactorCompensation = 0;
	float accelFactorCompensation = 0;
	float weightCorrected;
	float calCorrectionWeightFactor = 1;
	int order;

	if (compensationSpeed > 0){
		speedFactorCompensation = SPD_FACTOR_POS;
	} else if (compensationSpeed < 0) {
		speedFactorCompensation = SPD_FACTOR_NEG;
	}

/*
	if (!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)) {
		if (sizeof(realWeight) / sizeof(double) != 0){
			if (sizeof(realWeight) / sizeof(double) == 1){
				calCorrectionWeightFactor=realWeight[1]/weightADMeasured1stStage[1];
			} else if (sizeof(realWeight) / sizeof(double) > 2){
				order = 2;
				fitCurve(order, sizeof(weightADMeasured1stStage) / sizeof(double), weightADMeasured1stStage, realWeight, order + 1, coeffs);
				calCorrectionWeightFactor = evaluatePolynomial(weightToCorrect, coeffs, order)/weightToCorrect;
			} else if (sizeof(realWeight) / sizeof(double) == 2){
				order = 1;
				fitCurve(order, sizeof(weightADMeasured1stStage) / sizeof(double), weightADMeasured1stStage, realWeight, order + 1, coeffs);
				calCorrectionWeightFactor = evaluatePolynomial(weightToCorrect, coeffs, order)/weightToCorrect;
			}
		}
	}
	if (measureHeightValid || bitRead(functionBits, STAGE_TEMP_BIT)) {
		if (sizeof(realWeight) / sizeof(double) != 0){
			if (sizeof(realWeight) / sizeof(double) == 1){
				calCorrectionWeightFactor=realWeight[1]/weightADMeasured2ndStage[1];
			} else if (sizeof(realWeight) / sizeof(double) > 2){
				order = 2;
				fitCurve(order, sizeof(weightADMeasured2ndStage) / sizeof(double), weightADMeasured2ndStage, realWeight, order + 1, coeffs);
				calCorrectionWeightFactor = evaluatePolynomial(weightToCorrect, coeffs, order)/weightToCorrect;
			} else if (sizeof(realWeight) / sizeof(double) == 2){
				order = 1;
				fitCurve(order, sizeof(weightADMeasured2ndStage) / sizeof(double), weightADMeasured2ndStage, realWeight, order + 1, coeffs);
				calCorrectionWeightFactor = evaluatePolynomial(weightToCorrect, coeffs, order)/weightToCorrect;
			}
		}
	}
*/

//	double newWeightFromSensor = evaluatePolynomial(calCorrectionWeightFactor)


	if (compensationAcceleration > 0) {
		accelFactorCompensation = ACCEL_FACTOR_POS;
	} else if (compensationAcceleration < 0) {
		accelFactorCompensation = ACCEL_FACTOR_NEG;
	}

	calCorrectionWeightFactor = abs(calCorrectionWeightFactor); // removing any signal from correction factor
	weightCorrected = (weightToCorrect*calCorrectionWeightFactor + speedFactorCompensation * compensationSpeed + accelFactorCompensation * compensationAcceleration);

/*
	Serial.print(weightToCorrect*calCorrectionWeightFactor);
	Serial.print(" = (");
	Serial.print(weightToCorrect);
	Serial.print("*");
	Serial.print(calCorrectionWeightFactor);
	Serial.print(")		");

	Serial.print(weightCorrected);
	Serial.print(" = (");
	Serial.print(weightToCorrect*calCorrectionWeightFactor);
	Serial.print(" + ");
	Serial.print(speedFactorCompensation);
	Serial.print("*");
	Serial.print(compensationSpeed);
	Serial.print(" + ");
	Serial.print(accelFactorCompensation);
	Serial.print("*");
	Serial.println(compensationAcceleration);
	Serial.print(")");

*/
	return  weightCorrected;
}

float calculateCurrentWeight(){
	int order = 2;
	float calculatedWeight = 0;
	if (!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)) {
		fitCurve(order, sizeof(V1_AD) / sizeof(double), const_cast<double*>(V1_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);
	}
	if (measureHeightValid || bitRead(functionBits, STAGE_TEMP_BIT)) {
		fitCurve(order, sizeof(V2_AD) / sizeof(double), const_cast<double*>(V2_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);
	}
	calculatedWeight = evaluatePolynomial(weightSensorValue, coeffs, order);
	double* weightCoeffs = coeffs;
	int weightOrder = order;

	calculatedWeight = getAdjustedtedWeight(calculatedWeight, smoothedSpeed, acceleration, weightOrder, weightCoeffs); //new test line to try to correct weight by the speed
/*
	if (calculatedWeight < 0){
		return 0;
	}
	else{
*/
		return calculatedWeight;
//	}
}

// The setup function is called once at startup of the sketch
void setup() {
	// Initialize the LED strip
	FastLED.addLeds<WS2812, DATA_PIN, GRB>(leds, NUM_LEDS);
	FastLED.clear();
	// lastPulseTime = micros();
	// Add your initialization code here
	Serial.begin(115200);
	while (!Serial);

	/*
	cli();                    // Disable interrupts
	TCCR1A = 0;               // Set TCCR1A register to 0
	TCCR1B = 0;               // Set TCCR1B register to 0
	TCNT1 = 0;                // Set TCNT1 counter to 0
	OCR1A = 15999;            // Set comparison value for OCR1A register to 15999
	TCCR1B |= (1 << WGM12);   // Set Timer1 mode to CTC
	TCCR1B |= (1 << CS10);    // Set Timer1 prescaler to 1
	TIMSK1 |= (1 << OCIE1A);  // Enable Timer1 compare interrupt
	sei();                    // Enable interrupts
	 */


	// Save arrays to EEPROM if addresses are empty
	/*
	saveArrayToEeprom(V1_AD, V1_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
	saveArrayToEeprom(V2_AD, V2_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
	saveArrayToEeprom(WT_FIRST_SAMPLES, W_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
	saveFirstStartPosition(FIRST_START_POSITION_MM);
	 */
	// Read start position from EEPROM
	/*
    EEPROM.get(START_POSITION_ADDRESS, startPositionMm);
    if (isnan(startPositionMm) || startPositionMm == 0) {
        startPositionMm = FIRST_START_POSITION_MM; // Default value if EEPROM is empty or uninitialized
    }
	 */
	startPositionMm = FIRST_START_POSITION_MM;

	// Read arrays from EEPROM
	/*
    readArrayFromEeprom(V1_AD, V1_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
    readArrayFromEeprom(V2_AD, V2_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
    readArrayFromEeprom(WT_FIRST_SAMPLES, W_START_ADDR, ARRAY_SIZE_WEIGHT_SENSOR);
	 */
	// Print arrays to Serial Monitor
	printArray(const_cast<double*>(V1_AD), ARRAY_SIZE_WEIGHT_SENSOR, "V1");
	printArray(const_cast<double*>(V2_AD), ARRAY_SIZE_WEIGHT_SENSOR, "V2");
	printArray(const_cast<double*>(WT_FIRST_SAMPLES), ARRAY_SIZE_WEIGHT_SENSOR, "W");

	// Encoder settings
	pinMode(SIGN_A_PIN, INPUT);  // Signal A
	pinMode(SIGN_B_PIN, INPUT);  // Signal B
	attachInterrupt(digitalPinToInterrupt(SIGN_A_PIN), encoderA, RISING);
	attachInterrupt(digitalPinToInterrupt(SIGN_B_PIN), encoderB, RISING);

	// Pin definitions
	pinMode(LED_BUILTIN, OUTPUT);
	pinMode(WEIGHT_SENSOR_PIN_INPUT, INPUT);
	pinMode(BUTTON1, INPUT_PULLUP);
	pinMode(BUTTON2, INPUT_PULLUP);
	bitClear(functionBits, STAGE_VALID_BIT);
	bitClear(functionBits, STAGE_TEMP_BIT);

	// Proximity sensor settings
	pinMode(PROXIMITY_SENSOR, INPUT);
	attachPCINT(digitalPinToPCINT(PROXIMITY_SENSOR), proximitySensorISR, CHANGE);


	// Output for the lifting
	pinMode(LIFTING_OUTPUT, OUTPUT);

	//    countAnalogSums1 = 0;
	roundedWeight50Kg = 0;
	heightLimit = truckHeightLimit;  //set the start Height limitation. this value is calculated in the program

	Serial.print("Start Position: ");
	Serial.print(startPositionMm);
	Serial.println(" mm");

	previousLedStripTime = millis();
	lastLedStripTime = millis();
	//	lastSpeedSmoothTime = millis();
	lastWeightSmoothTime = millis();
	lastShowTime = millis();
	lastTimeAnalogRead = millis();
	lastDebounceTime1 = millis();
	lastProxSensorHighTime = millis();

	measureHeightValid = false;
	// Call simple linear regression algorithm
	char buf[100];
	int order = MAX_POLYNOMIAL_ORDER;  // Change this to the desired order
	snprintf(buf, 100, "Fitting curve of order %i...\n", order);
	Serial.print(buf);

	coeffs = new double[order + 1];  // Allocate memory for coefficients


	int originalOrder = 2;
	double v1stRef[] = {98, 372};
	double v2ndRef[] = {131, 411};
	double wtRef[] = {0, 1392};
	v1AdjustedAD = new double[ARRAY_SIZE_WEIGHT_SENSOR];
	v2AdjustedAD = new double[ARRAY_SIZE_WEIGHT_SENSOR];
	int referOrder = 1;
    int originalSize = sizeof(const_cast<double*>(V1_AD)) / sizeof(double);
	int refSize = sizeof(wtRef)/sizeof(wtRef[0]);

	Serial.print("originalSize: ");
	Serial.println(originalSize);
 	Serial.print("refSize: ");
	Serial.println(refSize);

	printArray(v1stRef, refSize, "v1stRef");
	printArray(v2ndRef, refSize, "v2ndRef");
	printArray(wtRef, refSize, "wtRef");
	delay(200);

	adjustXValues(const_cast<double*>(V1_AD), const_cast<double*>(WT_FIRST_SAMPLES), ARRAY_SIZE_WEIGHT_SENSOR, originalOrder, v1stRef, wtRef, refSize, referOrder, v1AdjustedAD);
    adjustXValues(const_cast<double*>(V2_AD), const_cast<double*>(WT_FIRST_SAMPLES), ARRAY_SIZE_WEIGHT_SENSOR, originalOrder, v2ndRef, wtRef, refSize, referOrder, v2AdjustedAD);

	printArray(v1AdjustedAD, ARRAY_SIZE_WEIGHT_SENSOR, "v1AdjustedAD");
	printArray(v2AdjustedAD, ARRAY_SIZE_WEIGHT_SENSOR, "v2AdjustedAD");


	order = 2;
	int ret = fitCurve(order, sizeof(V1_AD) / sizeof(double), const_cast<double*>(V1_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);

	printFormula(ret, order, "First stage Calculated coefficients:");
/*	Serial.println("First stage Calculated coefficients:");

	if (ret == 0) {  // Returned value is 0 if no error
		Serial.print("f(x) = ");
		for (int i = 0; i <= order; i++) {
			if (i > 0 && coeffs[i] >= 0) {
				Serial.print(" + ");
			}
			if (i == order) {
				Serial.print(coeffs[i], 4);
			} else {
				Serial.print(coeffs[i], 4);
				Serial.print(" * x");
				if (order - i > 1) {
					Serial.print("^");
					Serial.print(order - i);
				}
			}
		}
		Serial.println();
	}
*/

	order = 2;
	ret = fitCurve(order, sizeof(V2_AD) / sizeof(double), const_cast<double*>(V2_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);

	printFormula(ret, order, "Second stage Calculated coefficients:");
	/*
	Serial.println("Second stage Calculated coefficients:");
	if (ret == 0) {  // Returned value is 0 if no error
		Serial.print("f(x) = ");
		for (int i = 0; i <= order; i++) {
			if (i > 0 && coeffs[i] >= 0) {
				Serial.print(" + ");
			}
			if (i == order) {
				Serial.print(coeffs[i], 4);
			} else {
				Serial.print(coeffs[i], 4);
				Serial.print(" * x");
				if (order - i > 1) {
					Serial.print("^");
					Serial.print(order - i);
				}
			}
		}
		Serial.println();
	}
*/
	order = 3;
	//	ret = fitCurve(order, sizeof(weightSampleDpx) / sizeof(double), (tpxType) ? weightSampleTpx : weightSampleDpx, (tpxType) ? heightSampleTpx : heightSampleDpx, order + 1, coeffs);
	ret = fitCurve(order, sizeof(weightSampleTpx) / sizeof(double), weightSampleTpx, heightSampleTpx, order + 1, coeffs);
	printFormula(ret, order, "Weight Calculated coefficients:");
/*	Serial.println("Weight Calculated coefficients:");

	if (ret == 0) {  // Returned value is 0 if no error
		Serial.print("f(x) = ");
		for (int i = 0; i <= order; i++) {
			if (i > 0 && coeffs[i] >= 0) {
				Serial.print(" + ");
			}
			Serial.print(coeffs[i], 10);
			if (i < order) {
				Serial.print(" * x");
				if (order - i > 1) {
					Serial.print("^");
					Serial.print(order - i);
				}
			}
		}
		Serial.println();
	}
*/

	//	lastWeight = 0;
	smoothedWeight = 0;
	delay(200);
	lastForkPosition = (encoderPosition * PROFILE_PITCH * DRIVE_PULLEY_TEETH / PULSES_PER_REVOLUTION) * ENCODER_FACTOR_RELATION;

}

// The loop function is called in an endless loop
void loop() {
	/*
	int data1;
	unsigned int data2;
	 */

	blinkLed();
	buttonActuation();
	readAnalogInputs();  // Function to read input at each interrupt execution
	proximitySensorHandle();
	float weightSmoothFactor = 0;

	unsigned long currentTime = millis();

	int currentEncoderPosition = encoderPosition;

	// Check if the start position should be set
	if (abs(currentEncoderPosition - lastEncoderPosition) >= MIN_ENC_COUNTER) {
		lastEncoderPosition = currentEncoderPosition;

		if (directionUp && (proxSensCurrentLvl == PROX_SENSOR_LVL_LOGIC)) {
			if (currentTime - lastProxSensorHighTime >= PROX_SENSOR_VLD_WDTH) {
				measureHeightValid = true;
				bitSet(functionBits, STAGE_VALID_BIT);
				Serial.println("Start position set due to upward movement and proximity sensor trigger.");
				lastProxSensorHighTime = currentTime;
				encoderPosition = 0;
				positionMm = startPositionMm + ((encoderPosition * PROFILE_PITCH * DRIVE_PULLEY_TEETH / PULSES_PER_REVOLUTION) * ENCODER_FACTOR_RELATION);
				lastLedStripTime = currentTime;
				previousLedStripTime = currentTime;
				lastSpeedTime = currentTime;
				lastForkPosition = positionMm;
			}
		} else if (!directionUp && (proxSensCurrentLvl == PROX_SENSOR_LVL_LOGIC)) {
			if (currentTime - lastProxSensorHighTime >= PROX_SENSOR_VLD_WDTH) {
				measureHeightValid = false;
				bitClear(functionBits, STAGE_VALID_BIT);
				//        positionMm = 0.0;
				Serial.println("Position reset due to downward movement and proximity sensor trigger.");
				lastProxSensorHighTime = currentTime;
			}
		}
	}/* else {
		bitClear(functionBits, STAGE_TEMP_BIT);
	}
	 */
	// Update positionMm based on encoderPosition
	//float factor = tpxType ? ENCODER_TPX_FACTOR_RELATION : ENCODER_DPX_FACTOR_RELATION; // encoder factor

	currentTime = millis();

	if (measureHeightValid) {
		bitSet(functionBits, STAGE_VALID_BIT);
		positionMm = startPositionMm + (encoderPosition * PROFILE_PITCH * DRIVE_PULLEY_TEETH / PULSES_PER_REVOLUTION) * ENCODER_FACTOR_RELATION;
	} else {
		bitClear(functionBits, STAGE_VALID_BIT);
		positionMm = 0.0;
	}

	// Smooth and calculate speed
	currentTime = millis();
	float currentForkPosition = positionMm;
	//	float currentForkPosition = (encoderPosition * PROFILE_PITCH * DRIVE_PULLEY_TEETH / PULSES_PER_REVOLUTION) * ENCODER_FACTOR_RELATION;

	//	if (currentTime - lastSpeedSmoothTime >= SAMPLING_SPEED_PERIOD) {
	if (currentTime - lastSpeedTime >= SAMPLING_SPEED_PERIOD) {
		speed = calculateSpeed((currentForkPosition - lastForkPosition), (currentTime - lastSpeedTime) / 1000);  // division per 1000 is to convert mm/ms to mm/s
		currentTime = millis();
		if (speed <= LOW_SPEED_NEAR_ZERO) bitClear(functionBits, STAGE_TEMP_BIT); //clear bit that detects encoder counting


		// Smooth speed using EMA smoothing
		//        smoothedSpeed = ema(smoothedSpeed, speed, SMOOTH_FACTOR);  // Adjust the values to calculate smoothing factor as needed

		// ema factor speed near 0
		float lastSmoothedSpeed = smoothedSpeed;
		if (abs(smoothedSpeed) < LOW_SPEED_THRESHOLD && (abs(speed) > LOW_SPEED_NEAR_ZERO || abs(smoothedSpeed) > LOW_SPEED_NEAR_ZERO)) {
			smoothedSpeed = ema(smoothedSpeed, speed, EMA_SPD_FACTOR_NEAR_ZERO);
		}
		// ema factor speed higher than near zero
		else if (abs(smoothedSpeed) >= LOW_SPEED_THRESHOLD) {
			smoothedSpeed = ema(smoothedSpeed, speed, EMA_SPD_FACTOR_NORMAL);
		}
		// Check if speed is below force zero threshold
		if (abs(smoothedSpeed) < LOW_SPEED_NEAR_ZERO) {
			smoothedSpeed = 0;
		}
		float prevAccel = acceleration;
		//		float lastSpeed = smoothedSpeed;
		float currentAcceleration = calculateAcceleration((smoothedSpeed-lastSmoothedSpeed),((currentTime - lastSpeedTime) / 1000));
		//		acceleration = (prevAccel < LOW_SPEED_NEAR_ZERO ? currentAcceleration : ema(prevAccel, currentAcceleration, EMA_SPD_FACTOR_NEAR_ZERO));
		acceleration = ema(prevAccel, currentAcceleration, EMA_SPD_FACTOR_NORMAL);

		//		lastSpeedSmoothTime = currentTime;
		lastSpeedTime = currentTime;
		lastForkPosition = currentForkPosition;
	}

	/*
	if (bitRead(functionBits,ANALOG_READ)) {
		bitClear(functionBits,ANALOG_READ);
		if ((!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)) || measureHeightValid) currentWeight = calculateCurrentWeight();
	}
	 */


	// Calculate weight, smoothed weight and also round weight each 50kg. The section also actuate on enable/disable LIFTING_OUTPUT
	currentTime = millis();
	if (currentTime - lastWeightSmoothTime >= SAMPLING_WEIGHT_PERIOD) {
		lastWeightSmoothTime = currentTime;
		if (bitRead(functionBits,ANALOG_READ)) {
			bitClear(functionBits,ANALOG_READ);
			if ((!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)) || (measureHeightValid && currentTime - lastProxSensorHighTime >= IGNORE_AFTER_START_2ND_STAGE)){
				currentWeight = calculateCurrentWeight();
			}
		}
		//		Serial.println(currentWeight);

		float lastSmoothedWeight = smoothedWeight;
		//		if (currentWeight-lastSmoothedWeight >= DIFF_WEIGH_FORCE_SMOOTH) smoothedWeight = currentWeight;

		//		if ((abs(smoothedWeight) < LOW_WEIGHT_THRESHOLD && (abs(currentWeight) > LOW_WEIGHT_NEAR_ZERO || abs(smoothedWeight) > LOW_WEIGHT_NEAR_ZERO)) || (currentWeight - lastSmoothedWeight >= DIFF_WEIGH_FAST_SMOOTH && measureHeightValid) || (abs(currentWeight - lastSmoothedWeight)  >= DIFF_WEIGH_FORCE_SMOOTH && (!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)))) {
		if ((currentWeight - lastSmoothedWeight >= DIFF_WEIGH_FAST_SMOOTH && measureHeightValid) || (abs(currentWeight - lastSmoothedWeight)  >= DIFF_WEIGH_FORCE_SMOOTH && (!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT)))) {
			smoothedWeight = ema(smoothedWeight, (currentWeight <= LOW_WEIGHT_NEAR_ZERO ? 0 : currentWeight), EMA_WT_FACTOR_NEAR_ZERO);
//			Serial.println("EMA_WT_FACTOR_NEAR_ZERO");
//			smoothedWeight = ema(smoothedWeight, currentWeight, EMA_WT_FACTOR_NEAR_ZERO);
		}
		// ema factor speed higher than near zero
		else /*if (abs(smoothedWeight) >= LOW_WEIGHT_THRESHOLD || smoothedWeight - lastSmoothedWeight < DIFF_WEIGH_FAST_SMOOTH)*/ {
			smoothedWeight = ema(smoothedWeight, (currentWeight <= LOW_WEIGHT_NEAR_ZERO ? 0 : currentWeight), EMA_WT_FACTOR_NORMAL);
//			Serial.println("EMA_WT_FACTOR_NORMAL");
//			smoothedWeight = ema(smoothedWeight, currentWeight, EMA_WT_FACTOR_NORMAL);
		}

/*
		Serial.print(currentWeight);
		Serial.print("/");
		Serial.print(lastSmoothedWeight);
		Serial.print("/");
		Serial.print(DIFF_WEIGH_FORCE_SMOOTH);
		Serial.print("/");
		Serial.println(!measureHeightValid && !bitRead(functionBits, STAGE_TEMP_BIT));
*/

		// Check if speed is below force zero threshold
/*
		if (abs(smoothedWeight) < LOW_WEIGHT_NEAR_ZERO) {
			smoothedWeight = 0;
		}
*/

		//		roundedWeight50Kg = round((round(smoothedWeight / FIRST_STEP_ROUND) * FIRST_STEP_ROUND) / 50) * 50;
		roundedWeight50Kg = round(smoothedWeight / 50) * 50;
		/*
		data1 = roundedWeight50Kg;
		data2 = currentMillis3;
		 */


		// Calculate correspondent height limitation according to the weight
		int order = 3;  //define the polynomial order before coeffs calculation
		//		fitCurve(order, sizeof(weightSampleDpx) / sizeof(double), weightSampleDpx, (tpxType) ? heightSampleTpx : heightSampleDpx, order + 1, coeffs);
		fitCurve(order, sizeof(weightSampleTpx) / sizeof(double), weightSampleTpx, heightSampleTpx, order + 1, coeffs);
		double calculatedHeightLimit = evaluatePolynomial(roundedWeight50Kg, coeffs, order);
		heightLimit = calculatedHeightLimit-SECURITY_HEIGHT_LIMIT;
		if (heightLimit < 0){
			heightLimit = 0;
		} else if (heightLimit > truckH3){
			heightLimit = truckH3;
		}

		//  int mappedHeightLimit = map((positionMm < heightLimit - differenceGreenHeightValue ? heightLimit - differenceGreenHeightValue : positionMm > heightLimit ? heightLimit : positionMm),(heightLimit - differenceGreenHeightValue), heightLimit, 0, 255);
		//		int mappedHeightLimit = (positionMm <= heightLimit - DIFF_GREEN_H_VALUE) ? 0 : (positionMm >= heightLimit) ? 255 : map(positionMm, heightLimit - DIFF_GREEN_H_VALUE, heightLimit, 0, 255);  //the line up were change suggest by Copilot to this current line
		int mappedHeightLimit;
		if (positionMm <= heightLimit - DIFF_GREEN_H_VALUE) {
			mappedHeightLimit = 0;
		} else if (positionMm >= heightLimit || digitalRead(LIFTING_OUTPUT) == (liftLogicalOutputEnableHigh ? LOW : HIGH)) {
			mappedHeightLimit = 255;
		} else {
			mappedHeightLimit = map(positionMm, heightLimit - DIFF_GREEN_H_VALUE, heightLimit, 0, 255);
		}

		ledStripColor = CRGB(mappedHeightLimit, 255 - mappedHeightLimit, 0);

		if (positionMm >= (heightLimit) || positionMm >= truckHeightLimit) {
			digitalWrite(LIFTING_OUTPUT, (liftLogicalOutputEnableHigh ? LOW : HIGH));
		} else if ((digitalRead(LIFTING_OUTPUT) == (liftLogicalOutputEnableHigh ? LOW : HIGH) && (positionMm < (heightLimit-HEIGHT_HYSTERESIS))) || positionMm == 0){
			/*
			Serial.println(digitalRead(LIFTING_OUTPUT));
			Serial.println(heightLimit);
			Serial.println(positionMm);
			 */
			digitalWrite(LIFTING_OUTPUT, (liftLogicalOutputEnableHigh ? HIGH : LOW));
		}
	}

	currentTime = millis();
	//	if (currentTime < lastLedStripTime) lastLedStripTime = currentTime;
	if (currentTime - lastLedStripTime > LED_STRIP_FREQUENCY_CHECK) {
		lastLedStripTime = currentTime;
		float convertToLEDSpeed = LED_SPEED_FACTOR / abs(smoothedSpeed); //= (abs(smoothedSpeed) > 0 ? LED_SPEED_FACTOR / abs(smoothedSpeed) : 2000); substituted to try the wavePattern without "converting the value 0
		if ((convertToLEDSpeed * LED_SPEED_FACTOR > LOW_SPEED_NEAR_ZERO) && measureHeightValid) {
			if (currentTime - previousLedStripTime >= (convertToLEDSpeed)) {
				previousLedStripTime = currentTime;
				wavePattern(ledStripColor, convertToLEDSpeed, directionUp);  // Green ledStripColor, speed of ledSpeed, specified direction
			}
		} else if (currentTime - previousLedStripTime > TIMER_INTERVAL_2000MS/LED_STRIP_FREQUENCY_CHECK) {
			previousLedStripTime = currentTime;
			fill_solid(leds, NUM_LEDS, ledStripColor);
			FastLED.show();
		}
	}

	// Display some current values speed (smoothed), position, weight, height limit every showPeriod milliseconds
	currentTime = millis();
	if (currentTime - lastShowTime >= SHOW_PERIOD) {
		lastShowTime = currentTime;

		int order = 2;
		float calculatedWeight = 0;
		if (!bitRead(functionBits, STAGE_VALID_BIT) || !bitRead(functionBits, STAGE_TEMP_BIT)) {
			fitCurve(order, sizeof(V1_AD) / sizeof(double), const_cast<double*>(V1_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);
		}
		if (bitRead(functionBits, STAGE_VALID_BIT) || bitRead(functionBits, STAGE_TEMP_BIT)) {
			fitCurve(order, sizeof(V2_AD) / sizeof(double), const_cast<double*>(V2_AD), const_cast<double*>(WT_FIRST_SAMPLES), order + 1, coeffs);
		}
		calculatedWeight = evaluatePolynomial(weightSensorValue, coeffs, order);

/*
		if (calculatedWeight < 0){
			calculatedWeight = 0;
		}
*/

		Serial.print("LimitHt: ");
		Serial.print(heightLimit);
		Serial.print(", Position: ");
		if (positionMm == 0.0) {
			Serial.print("-");
		} else {
			Serial.print(positionMm);
			Serial.print(" mm");
		}

		Serial.print(", STAGE_TEMP_BIT: ");
		Serial.print(bitRead(functionBits, STAGE_TEMP_BIT));

		Serial.print(", WeightSens: ");
		Serial.print(weightSensorValue);

		Serial.print(", EncPos: ");
		Serial.print(encoderPosition);

		Serial.print(", SPD: ");
		Serial.print(smoothedSpeed);

		Serial.print(", Accel: ");
		Serial.print(acceleration);

		Serial.print(", CalcWT: ");
		Serial.print(calculatedWeight);

		Serial.print(", Simul WT ");
		if (!bitRead(functionBits, STAGE_VALID_BIT)) {
			Serial.print("1°: ");
		}
		if (bitRead(functionBits, STAGE_VALID_BIT)) {
			Serial.print("2°: ");
		}
		Serial.print(roundedWeight50Kg);
		Serial.print("/");

		Serial.print(smoothedWeight);
		Serial.print("/");

		Serial.print(currentWeight);
		Serial.print("kg			");
//		plot(speed,acceleration,(!bitRead(functionBits, STAGE_VALID_BIT)?1:2),weightSensorValue,calculatedWeight,smoothedWeight);
		Serial.println();



		/*
		Serial.print("Speed: ");

		if (abs(smoothedSpeed) == 0.000) {
			Serial.print("-");
		} else {
			Serial.print(smoothedSpeed, 2);
			Serial.print(" mm/s, ");
			if (directionUp) {
				Serial.print("Going up");
			} else {
				Serial.print("Going down");
			}
		}
		Serial.print(", Residual Limit Height: ");
		Serial.print(heightLimit);


		Serial.print(", Weight sensor reading: ");
		Serial.print(weightSensorValue);

				Serial.print(", ProxySwitch: ");
		Serial.print(digitalRead(PROXIMITY_SENSOR));

		Serial.print(", Encoder Position: ");
		Serial.print(encoderPosition);

		Serial.print(", Position: ");
		if (positionMm == 0.0) {
			Serial.println("-");
		} else {
			Serial.print(positionMm);
			Serial.println(" mm");
		}



		 * Print weight estimative using Serial.print
		 * Include or remove the comment block to disable/enable weight printing



		Serial.print("Simulação peso médio/real ");
		if (!bitRead(functionBits, STAGE_VALID_BIT)) {
			Serial.print("1° estágio: ");
		}
		if (bitRead(functionBits, STAGE_VALID_BIT)) {
			Serial.print("2° estágio: ");
		}
		Serial.print(roundedWeight50Kg);
		Serial.print("/");

		Serial.print(smoothedWeight);
		Serial.print("/");

		Serial.print(currentWeight);
		Serial.println("kg");
		 */
	}


	currentTime = millis();
}