#include <Arduino.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Explicit Pin definitions
#define PIN_STEP          D11   
#define PIN_DIR           D10   
#define PIN_ESTOP         D2   
#define PIN_START         D4   
#define PIN_RESET         D8   
#define PIN_SYS_OK        D5   
#define PIN_TRIP          D3   

// 3-Stage Hardware Color Health Array
#define PIN_HLTH_GRN      A4   
#define PIN_HLTH_YLW      A5  
#define PIN_HLTH_RED      D9   

// Encoder Interface Parameters
#define PIN_ENC_SW        D6   
#define PIN_ENC_CLK       D7   
#define PIN_ENC_DT        D8   

// Analog Inputs
#define PIN_CURRENT       A0   
#define PIN_VIBRATION     A1   
#define PIN_TEMP          A2   
#define PIN_ACOUSTIC      A3   

LiquidCrystal_I2C lcd(0x27, 20, 4);

typedef enum {
    STATE_READY,
    STATE_RUNNING,
    STATE_TRIPPED
} SystemState_t;

SystemState_t current_state = STATE_READY;
const char* trip_reason = "";
unsigned long last_lcd_update = 0;

bool motor_forward = true;
long encoder_pulses = 0;
int last_clk_state = HIGH;

void updateHardwareHealthLamps(int score) {
    if (current_state == STATE_TRIPPED) {
        digitalWrite(PIN_HLTH_GRN, LOW);  digitalWrite(PIN_HLTH_YLW, LOW);  digitalWrite(PIN_HLTH_RED, HIGH); 
        return;
    }
    if (score >= 75) {
        digitalWrite(PIN_HLTH_GRN, HIGH);   digitalWrite(PIN_HLTH_YLW, LOW);    digitalWrite(PIN_HLTH_RED, LOW);
    } else if (score >= 40 && score < 75) {
        digitalWrite(PIN_HLTH_GRN, LOW);    digitalWrite(PIN_HLTH_YLW, HIGH);   digitalWrite(PIN_HLTH_RED, LOW);
    } else {
        digitalWrite(PIN_HLTH_GRN, LOW);    digitalWrite(PIN_HLTH_YLW, LOW);    digitalWrite(PIN_HLTH_RED, HIGH);
    }
}

// Fixed static background template mapping 
void drawLCDStaticLayout() {
    lcd.clear();
    lcd.setCursor(0, 0); lcd.print("CBM:STANDBY HLTH:---");
    lcd.setCursor(0, 1); lcd.print("I:0A       V:0g     ");
    lcd.setCursor(0, 2); lcd.print("  T:0C     A:0kHz   ");
    lcd.setCursor(0, 3); lcd.print("DIR:FWD  POS:0      ");
}

void setup() {
    pinMode(PIN_STEP, OUTPUT);
    pinMode(PIN_DIR, OUTPUT);
    pinMode(PIN_SYS_OK, OUTPUT);
    pinMode(PIN_TRIP, OUTPUT);
    
    pinMode(PIN_HLTH_GRN, OUTPUT);
    pinMode(PIN_HLTH_YLW, OUTPUT);
    pinMode(PIN_HLTH_RED, OUTPUT);
    
    pinMode(PIN_ESTOP, INPUT_PULLUP);
    pinMode(PIN_START, INPUT_PULLUP);
    pinMode(PIN_RESET, INPUT_PULLUP);
    
    pinMode(PIN_ENC_SW, INPUT_PULLUP);
    pinMode(PIN_ENC_CLK, INPUT_PULLUP);
    pinMode(PIN_ENC_DT, INPUT_PULLUP);
    
    lcd.init();
    lcd.backlight();
    drawLCDStaticLayout();

    digitalWrite(PIN_SYS_OK, HIGH);
    digitalWrite(PIN_TRIP, LOW);
    digitalWrite(PIN_DIR, HIGH); 
    
    last_clk_state = digitalRead(PIN_ENC_CLK);
}

void loop() {
    int raw_curr  = analogRead(PIN_CURRENT);
    int raw_vibe  = analogRead(PIN_VIBRATION);
    int raw_temp  = analogRead(PIN_TEMP);
    int raw_acou  = analogRead(PIN_ACOUSTIC);

    int current_A    = (raw_curr * 20) / 1023;  
    int vibration_g  = (raw_vibe * 6) / 1023;   
    int temp_C       = (raw_temp * 120) / 1023; 
    int acoustic_kHz = (raw_acou * 100) / 1023; 

    int health_score = 100 - (((current_A * 5) + (vibration_g * 16) + (temp_C * 1)) / 3);
    if (health_score < 0) health_score = 0;
    if (health_score > 100) health_score = 100;

    updateHardwareHealthLamps(health_score);

    // Read Shaft Encoder Ticks
    int current_clk_state = digitalRead(PIN_ENC_CLK);
    if (current_clk_state != last_clk_state && current_clk_state == LOW) {
        if (digitalRead(PIN_ENC_DT) != current_clk_state) {
            encoder_pulses++;
        } else {
            encoder_pulses--;
        }
    }
    last_clk_state = current_clk_state;

    // Toggle Direction
    if (current_state != STATE_TRIPPED && digitalRead(PIN_ENC_SW) == LOW) {
        motor_forward = !motor_forward;
        digitalWrite(PIN_DIR, motor_forward ? HIGH : LOW);
        
        lcd.setCursor(4, 3);
        lcd.print(motor_forward ? "FWD" : "REV");
        delay(250); 
    }

    if (digitalRead(PIN_ESTOP) == LOW) {
        current_state = STATE_TRIPPED;
        trip_reason = "MANUAL E-STOP     ";
    }

    switch (current_state) {
        case STATE_READY:
            digitalWrite(PIN_SYS_OK, HIGH);
            digitalWrite(PIN_TRIP, LOW);
            
            if (millis() - last_lcd_update >= 250) {
                last_lcd_update = millis();
                lcd.setCursor(4, 0); lcd.print("STANDBY "); // Clean overwrite padding strings
                lcd.setCursor(17, 0); lcd.print(health_score); lcd.print("% ");
                lcd.setCursor(13, 3); lcd.print(encoder_pulses); lcd.print("   ");
            }

            if (digitalRead(PIN_START) == LOW) {
                current_state = STATE_RUNNING;
                lcd.setCursor(4, 0); lcd.print("RUNNING "); // Clean shift statement
                delay(150);
            }
            break;

        case STATE_RUNNING:
            // Pulse Step generation to trigger motor execution
            digitalWrite(PIN_STEP, HIGH); delayMicroseconds(900);
            digitalWrite(PIN_STEP, LOW); delayMicroseconds(900);

            // Safety limits validation
            if (current_A > 15) { current_state = STATE_TRIPPED; trip_reason = "MOTOR OVERLOAD    "; }
            else if (vibration_g > 4) { current_state = STATE_TRIPPED; trip_reason = "BEARING ANOMALY   "; }
            else if (temp_C > 85) { current_state = STATE_TRIPPED; trip_reason = "THERMAL RUNAWAY   "; }
            else if (acoustic_kHz > 70) { current_state = STATE_TRIPPED; trip_reason = "STRUCTURAL CRACK  "; }

            // Concurrent refresh panel layout loops (250ms spacing)
            if (millis() - last_lcd_update >= 250) {
                last_lcd_update = millis();
                
                lcd.setCursor(4, 0);  lcd.print("RUNNING ");
                lcd.setCursor(17, 0); lcd.print(health_score); lcd.print("% ");
                
                // Line 1 alignment updates
                lcd.setCursor(2, 1);  lcd.print(current_A); lcd.print("A  ");
                lcd.setCursor(13, 1); lcd.print(vibration_g); lcd.print("g  ");
                
                // Line 2 alignment corrections (Fixed the broken custom Temp symbol overlap bug)
                lcd.setCursor(2, 2);  lcd.print(temp_C); lcd.print("C   ");
                lcd.setCursor(13, 2); lcd.print(acoustic_kHz); lcd.print("kHz ");
                
                // Line 3 update loops
                lcd.setCursor(13, 3); lcd.print(encoder_pulses); lcd.print("    ");
            }
            break;

        case STATE_TRIPPED:
            digitalWrite(PIN_SYS_OK, LOW);
            digitalWrite(PIN_STEP, LOW);
            updateHardwareHealthLamps(0); 
            
            lcd.setCursor(0, 0); lcd.print("!! CRITICAL TRIP  !!");
            lcd.setCursor(0, 1); lcd.print("REASON:             ");
            lcd.setCursor(0, 2); lcd.print(trip_reason);
            lcd.setCursor(0, 3); lcd.print("PRESS RESET TO CLEAR");

            while (current_state == STATE_TRIPPED) {
                digitalWrite(PIN_TRIP, HIGH); delay(200);
                digitalWrite(PIN_TRIP, LOW); delay(200);
                
                if (digitalRead(PIN_RESET) == LOW) {
                    encoder_pulses = 0; 
                    drawLCDStaticLayout();
                    current_state = STATE_READY;
                    delay(300);
                }
            }
            break;
    }
}