#include <Wire.h>
#include <LiquidCrystal.h>
#include <math.h>

// LCD configuration
//LiquidCrystal lcd(RS, RW, E, D0, D1, D2, D3, D4, D5, D6, D7);	/* For 8-bit mode */
// VSS = GBD; VDD=+5V; V0=10K_POT (+5V/GND); A = 220Ohm-(+5V); K = GND 
LiquidCrystal lcd(12, A2, 11, 6, 5, 13, A3, 10, 9, 8, 7);

// Analog/Digital pins 
const byte analogPinF = A0; // Forward power
const byte analogPinR = A1; // Reflected power
#define buttonMenuPin  4
#define buttonSelectPin 2

// Power measurement variables
int Vreference = 5000; // mV
const float adcToVoltage = Vreference / 1023.0f;
int adcValueF = 0;
int adcValueR = 0; 
float pow_fwd = 0;
float pow_ref = 0;
float peak_power = 0;
float peak_power_ref = 0;
float last_gamma = 0.0f;
float last_peak_power = 0.0f;
float swr = 1.0;
unsigned long last_peak_time = 0;
unsigned long last_swr_peak_reset = 0;
const unsigned long peak_hold_time = 1000; // Power And SWR Hold time

// Bar graph variables
static int last_lit_segments = -1;
static int last_peak_col = -1;
const int virtual_columns = 80;   // Each segment is devided by 5. For 1602A: 16x5=80.
float bar_level = 0;              // Do not change
const float bar_decay = 0.975;     // Decay rate (adjust in 0.95<bar_decay<0.99 range)
const float bar_threshold = 0.05; // Threshold when bars cleared out
float bar_graph = 60;            // Power Scale. For 100W SSB use bar_graph = 100*0.6 = 60
float bar_graph_mult = 1.0f / bar_graph; // Do not Chnage
const int change_threshold = 1; //Minimum number of segment changes required before the lcd display is updated
// Beak Logic
float peak_bar_level = 0;             // Peak holding bar level
float peak_bar_position = 0;          // Tracks the highest point reached
const float peak_bar_decay = 0.995;    // Slower decay for peak bar
unsigned long last_peak_update = 0;    // When peak was last updated
const unsigned long peak_hold_duration = 2000; // Hold peak time before decay

// Custom characters
// Peak Bar
byte Pbar = 0x01;
byte bars[6][8] = {
    {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, Pbar, 0x00},  // bar0
    {0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x10, 0x00},  // bar1
    {0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x18, 0x00},  // bar2
    {0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x1C, 0x00},  // bar3
    {0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x1E, 0x00},  // bar4
    {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x00}   // bar5
};  

byte peakMarker[8] = {Pbar, Pbar, Pbar, Pbar, Pbar, Pbar, Pbar, 0x00}; // A small horizontal line
//byte peakMarker[8] = {0x04,0x04,0x04,0x04,0x04,0x04,0x04,0x00}; // A small horizontal line

// Band calibration
typedef struct {
  const char* name;
  float fwdA;
  float fwdB;
  float refA;
  float refB;
} BandInfo;

BandInfo bands[8] = {
  {"Def", 0.02618, 5.2, 0.02718, 7.2},
  {"160m", 0.02618, 5.3, 0.02718, 7.3},
  {"80m", 0.02618, 5.4, 0.02718, 7.4},
  {"60-40m", 0.02618, 5.5,0.02718, 7.5},
  {"30-20m", 0.02618, 5.6,0.02718, 7.6},
  {"17-15m", 0.02618, 5.7,0.02718, 7.8},
  {"12-10m", 0.02618, 5.8,0.02718, 7.9},
  {"6m", 0.02618,5.9, 0.02718, 8.0}
};

int currentBand = 0;
bool menuMode = false;
bool showReflectedPower = false;
byte menuState = 0; // 0=main, 1=band select, 2=fwd/ref select
unsigned long lastButtonPress = 0;
const unsigned long debounceDelay = 200;
unsigned long menuStartTime = 0;
const int menu_timeout = 5000; // Timeout in milliseconds (5 seconds)

inline float fast_sqrt(float x) {
  union { float f; uint32_t i; } conv = {x};
  conv.i = 0x1fbd1df5 + (conv.i >> 1);
  float xhalf = 0.5f * x;
  conv.f *= 1.5f - (xhalf * conv.f * conv.f);
  return x * conv.f;
}

inline float pow10_fast(float x) {
  return expf(2.302585092994046f * x);
}

float computePower(int adcValue, bool isForward) {
  if (adcValue < 5) return 0.0f;  // Deadzone for noise
  float voltage = adcValue * adcToVoltage;
  float slope = isForward ? bands[currentBand].fwdA : bands[currentBand].refA;
  float intercept = isForward ? bands[currentBand].fwdB : bands[currentBand].refB;
  float powerdBm = (slope * voltage) - intercept;
  return pow10_fast((powerdBm - 30) * 0.1f);
}

void setup() {
  lcd.begin(16, 2);
    // Initialize all custom characters (0 to 5)
    for (int i = 0; i < 6; i++) {
        lcd.createChar(i, bars[i]);
    }
  lcd.createChar(6, peakMarker);

  pinMode(buttonMenuPin, INPUT_PULLUP);
  pinMode(buttonSelectPin, INPUT_PULLUP);

  lcd.setCursor(0, 0);
  lcd.print("Po:0W");
  lcd.setCursor(10, 0);
  lcd.print("SW:1.00");

  analogReference(DEFAULT);
  ADCSRA = (ADCSRA & 0xF8) | 0x02;
}

void loop() {
  handleButtons();

  if (!menuMode) {
    readAndUpdatePower();
    calculateSWR();
    updateBarGraph();
    updateDisplay();
  }
}

void readAndUpdatePower() {
    adcValueF = 0;
    adcValueR = 0;

    int adcValueFF = 0;
    int adcValueRR = 0;
    for(int i = 0; i <=20; i++) {
        adcValueFF = analogRead(analogPinF);
        adcValueRR = analogRead(analogPinR);
        adcValueF = max(adcValueFF , adcValueF);
        adcValueR = max(adcValueRR , adcValueR);
    }
  
    pow_fwd = computePower(adcValueF, true);
    pow_ref = computePower(adcValueR, false);

    // Always update both peaks for SWR calculation
    if (pow_fwd > peak_power) {
        peak_power = pow_fwd;
        last_peak_time = millis();
    } else if (millis() - last_peak_time > peak_hold_time) {
        peak_power = pow_fwd;
    }

    if (pow_ref > peak_power_ref) {
        peak_power_ref = pow_ref;
        last_swr_peak_reset = millis();
    }

    // For display purposes, track either forward or reflected power
    float display_power = showReflectedPower ? pow_ref : pow_fwd;
    if (display_power > peak_power) {
        peak_power = display_power;
        last_peak_time = millis();
    } else if (millis() - last_peak_time > peak_hold_time) {
        peak_power = display_power;
    }
}

void calculateSWR() {
    // Update peak reflected power if current reading is higher
    if (pow_ref > peak_power_ref) {
        peak_power_ref = pow_ref;
        last_gamma = -1.0f; // Force SWR recalculation
    }

    // Reset peak reflected power after hold time or if current reading is 0
    if (millis() - last_swr_peak_reset > peak_hold_time || pow_ref == 0) {
        peak_power_ref = pow_ref;
        last_swr_peak_reset = millis();
        last_gamma = -1.0f; // Force SWR recalculation
    }

    // Only calculate SWR if we have sufficient forward power
    if (peak_power > 0.1f) {  // At least 0.1W forward power
        if (last_gamma < 0 || peak_power != last_peak_power) {
            // Recalculate gamma (reflection coefficient)
            if (peak_power_ref > 0) {
                float ratio = peak_power_ref / peak_power;
                last_gamma = fast_sqrt(ratio);
            } else {
                last_gamma = 0.0f; // No reflected power = perfect match
            }
            last_peak_power = peak_power;
        }

        // Calculate SWR from gamma
        if (last_gamma >= 0 && last_gamma < 1.0f) {
            swr = (1.0f + last_gamma) / (1.0f - last_gamma);
        } else {
            swr = 99.9f; // Handle invalid cases
        }
    } else {
        // Insufficient forward power - assume perfect match
        swr = 1.0f;
        last_gamma = 0.0f;
    }

    // Ensure SWR is within valid range
    if (swr < 1.0f) swr = 1.0f;
    if (swr > 99.9f) swr = 99.9f;
}

void updateDisplay() {
  lcd.setCursor(0, 0);
  lcd.print(showReflectedPower ? "Pr:" : "Po:");
  
  int display_power = int(peak_power + 0.5);
  if (display_power < 1) {
    lcd.print("0W   ");
  } else {
    lcd.print(display_power);
    lcd.print("W   ");
  }

  lcd.setCursor(9, 0);
  lcd.print("SW:");
  lcd.print(swr, 2);
}

void handleButtons() {
  if (millis() - lastButtonPress < debounceDelay) return;

  if (digitalRead(buttonMenuPin) == LOW) {
    lastButtonPress = millis();
    
    if (!menuMode) {
      // Enter menu mode - Band Selection
      menuMode = true;
      menuState = 1;
      menuStartTime = millis();
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Select Band:");
      lcd.setCursor(0, 1);
      lcd.print(bands[currentBand].name);
    } 
    else {
      // Already in menu mode - cycle through menu states
      menuState++;
      
      if (menuState == 1) {
        // Band Selection
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Select Band:");
        lcd.setCursor(0, 1);
        lcd.print(bands[currentBand].name);
      }
      else if (menuState == 2) {
        // FWD/REF Selection
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Select Mode:");
        lcd.setCursor(0, 1);
        lcd.print(showReflectedPower ? "REF Power" : "FWD Power");
      }
      else {
        // Exit menu (state 3 or higher)
        menuMode = false;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(showReflectedPower ? "Pr:0W" : "Po:0W");
        lcd.setCursor(9, 0);
        lcd.print("SW:");
        lcd.print(swr, 2);
      }
    }
  }

  if (menuMode && digitalRead(buttonSelectPin) == LOW) {
    lastButtonPress = millis();
    menuStartTime = millis();  // Reset timeout timer
    
    if (menuState == 1) {
      // Band selection
      currentBand = (currentBand + 1) % 8;
      lcd.setCursor(0, 1);
      lcd.print("                ");
      lcd.setCursor(0, 1);
      lcd.print(bands[currentBand].name);
    }
    else if (menuState == 2) {
      // FWD/REF toggle
      showReflectedPower = !showReflectedPower;
      lcd.setCursor(0, 1);
      lcd.print("                ");
      lcd.setCursor(0, 1);
      lcd.print(showReflectedPower ? "REF Power" : "FWD Power");
    }
  }

  // AUTO EXIT after 5 seconds of inactivity
  if (menuMode && (millis() - menuStartTime > menu_timeout)) {
    menuMode = false;
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print(showReflectedPower ? "Pr:0W" : "Po:0W");
    lcd.setCursor(9, 0);
    lcd.print("SW:");
    lcd.print(swr, 2);
  }
}

void updateBarGraph() {
  // Update current bar level
  float power = showReflectedPower ? pow_ref : pow_fwd;

  float target_level = power * bar_graph_mult;
  bar_level = (target_level > bar_level) ? target_level : bar_level * bar_decay;
  
  // Update peak position
  if (target_level > peak_bar_position) {
    peak_bar_position = min(target_level, 16.0);  // Ensure it doesn't go beyond 16
    last_peak_update = millis();
  } else if (millis() - last_peak_update > peak_hold_duration) {
    peak_bar_position *= peak_bar_decay;
    peak_bar_position = constrain(peak_bar_position, 0, 16);  // Ensure it doesn't fall below 0
  }
  
  // Clear display if below threshold
  if (bar_level < bar_threshold && peak_bar_position < bar_threshold) {
    if (last_lit_segments != 0 || last_peak_col != -1) {
      last_lit_segments = 0;
      last_peak_col = -1;
      lcd.setCursor(0, 1);
      lcd.print("                ");  // Clear the line
    }
    return;
  }

  // Calculate current and peak positions
  int lit_segments = round(bar_level * virtual_columns);
  lit_segments = constrain(lit_segments, 0, virtual_columns);
  
  int peak_segments = round(peak_bar_position * virtual_columns);
  peak_segments = constrain(peak_segments, 0, virtual_columns);
  
  // Calculate the column where the peak marker should be placed
  int peak_col = peak_segments / 5;  // Which character column contains the peak
  peak_col = constrain(peak_col, 0, 15);  // Ensure it doesn't go beyond 15 (max column for LCD)

  // Only redraw if significant changes
  if (abs(lit_segments - last_lit_segments) >= change_threshold || 
      peak_col != last_peak_col ||
      lit_segments == virtual_columns || 
      lit_segments == 0) {
    
    last_lit_segments = lit_segments;
    last_peak_col = peak_col;
    
    // First draw the normal bar graph
    lcd.setCursor(0, 1);
    for (int i = 0; i < 16; i++) {
      int segments = lit_segments - (i * 5);
      byte char_num = 0;
      if (segments > 0) {
        char_num = (segments >= 5) ? 5 : segments;
      }
      lcd.write(char_num);
    }
    
    // Now overlay the peak marker if visible
    if (peak_bar_position >= bar_threshold && peak_col < 16) {
      lcd.setCursor(peak_col, 1);
      lcd.write(6); // Write the peak marker character
    }
  }
}