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

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

const int analogPinF = A0;
const int analogPinR = A1;

int Vreference = 5000;
float slopeF = 0.02618; // Slope of the AD8307 log output (Default = 40)
float interceptF = 45.2; // 0V intercept point (Default = 44)
float slopeR = 0.02618; // Slope of the AD8307 log output (Default = 40)
float interceptR = 45.2; // 0V intercept point (Default = 44)  

int adcValueF = 0;
int voltageF = 0;
double powerdBmF = 0;
double powerdBmR = 0;
int adcValueR = 0;
int voltageR = 0;
double vswr = 1.0;
double refl_coeff = 0;

byte Timing = 1;
byte ReflectedLCD = 1;
#define IN_Timing 2
#define Buzzer    3    //  output for Buzzer control  (1 = ON)
#define IN_ReflectedLCD 4

unsigned long   pow_fwd = 0;              // power forward (watts)
unsigned long   pow_ref = 0;              // power forward (watts)

// Peak Hold Variables
double peakPowerFwd = 0;
double peakPowerRef = 0;
unsigned long peakHoldTimer = 0;
int peakHoldTime = 0;  // Hold peak power
int peakHoldTime_Fast = 500;  // Hold peak power for 0.5 seconds
int peakHoldTime_Slow = 2000;  // Hold peak power for 2 seconds

void UpdateTXScreen();
void UpdateTXScreenRef();

// Progress Bar Characters
// Progress Bar Characters
byte block[5][8] = {  // Reduced from 6 to 5
    {0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000},
    {0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000},
    {0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100},
    {0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110},
    {0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111}
};

// Progress Bar Variables
unsigned long lastUpdateLCD = 0;
const unsigned long updateIntervalLCD = 16;  // LCD update interval

/////////////////// LDMOS parameters
byte  fill[6]={ 0x20,0x00,0x01,0x02,0x03,0xFF };      // character used to fill (0=empty  5=full)
byte  peak[7]={ 0x20,0x00,0x04,0x05,0x06,0x07,0x20 }; // character used to peak indicator
int   lmax[2];                                        // level max memory
int   dly[2];                                         // delay & speed for peak return
int smooth_lev[2] = {0, 0};  // One for each row
long  lastTpep = 0;                                   // update PEP display

int   anF = 0;                            // analog read forward power
int   anR = 0;                            // analog read reflected power
#define T_REFRESH1    5          // msec bargraph refresh rate
#define T_PEAKHOLD   200          // msec peak hold time before return
#define Fall_time 5

/////////////////// End of LDMOS parameters

// Fast 10^x approximation
inline float pow10_fast(float x) {
  return expf(2.302585092994046f * x); // exp(x * ln(10))
}

// Power calculation function
float computePower(int adcValue, float slope, float intercept) {
  float voltage = (adcValue / 1023.0f) * Vreference; // 'f' suffix for float optimization
  float powerdBm = (slope * voltage) - intercept;
  return pow10_fast((powerdBm - 30) * 0.1f); // Multiply by 0.1f is faster than /10
}

/// Time measure
long duration = 0;
long start = 0;
/// End of Time measure

void setup() {
  Serial.begin(9600);
  lcd.begin(16, 2);

    lcd.setCursor(9, 0);
    lcd.print("SW:1.00");

  analogReference(DEFAULT);
  pinMode(IN_Timing, INPUT_PULLUP);
  pinMode(IN_ReflectedLCD, INPUT_PULLUP);

  pinMode(Buzzer, OUTPUT);
  digitalWrite(Buzzer, 0);
  
  // Create custom characters
  for (int i = 0; i < 5; i++) {
   lcd.createChar( i,block[i] );
  }
  ADCSRA = (ADCSRA & 0xF8) | 0x04; // Set ADC clock divider to 16 (1MHz/16 = 62.5kHz)
}

void loop() {
  start = micros(); 

  // Read analog values ONCE per channel
  adcValueF = analogRead(analogPinF);
  adcValueR = analogRead(analogPinR);

  // Compute powers using the helper function
  pow_fwd = computePower(adcValueF, slopeF, interceptF);
  pow_ref = computePower(adcValueR, slopeR, interceptR);

 
  if (pow_fwd > 0 && pow_fwd > pow_ref ) {
    refl_coeff = abs(sqrt((double)pow_ref / pow_fwd)); 
    vswr = (1 + refl_coeff) / (1 - refl_coeff);
    }
    else {
      vswr = 1.00;
    }
 
/*
    if (pow_fwd <= pow_ref && pow_fwd > 0) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("E R R O R !");
    lcd.setCursor(0, 1);
    lcd.print("SWR = infinity");
    tone(Buzzer, 700);
    delay (200);    
     noTone(Buzzer);
    delay (200);
     return;
  }
  */

  /* 
  if (pow_fwd > 4000) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("R E D U C E");
    lcd.setCursor(0, 1);
    lcd.print("P O W E R !");
    tone(Buzzer, 700);
    delay (200);    
    noTone(Buzzer);
    delay (200);
    tone(Buzzer, 700);
    delay (200);    
    noTone(Buzzer);
    delay (200);
     return;
  }
  */

  Timing = digitalRead(IN_Timing);
  if (Timing == LOW){
  peakHoldTime = peakHoldTime_Slow;
  }
  else { 
   peakHoldTime = peakHoldTime_Fast; 
  }

/*
if (vswr >= 1.5){
    tone(Buzzer, 700);
    delay (200);    
    noTone(Buzzer);
    delay (200);
    tone(Buzzer, 700);
    delay (200);    
    noTone(Buzzer);
    delay (200);
}
*/

  if (millis() - lastUpdateLCD >= updateIntervalLCD) {
ReflectedLCD = digitalRead(IN_ReflectedLCD);
if(ReflectedLCD == HIGH){
UpdateTXScreen();
 }
 else{
  UpdateTXScreenREF();
 }
    lastUpdateLCD = millis();
  }

/////////////
 anF = map(pow_fwd, 0, 100, 0, 80);    // 80 = 16 x 5 colums, full scale 100w
 anR = map(pow_ref, 0, 100, 0, 80);    // 80 = 16 x 5 colums, full scale 100w
  
/////////////
duration = micros() - start;
Serial.println(duration);
//delay(1000);
}

void UpdateTXScreen() {
    // Peak Hold Logic
    if (pow_fwd > peakPowerFwd) {
      peakPowerFwd = pow_fwd;
      peakHoldTimer = millis();
    }

    if (millis() - peakHoldTimer > peakHoldTime) {
      peakPowerFwd = pow_fwd; // Update after hold time
    }

    lcd.setCursor(0, 0);
    lcd.print("Po:0W");
    lcd.setCursor(3, 0);
    lcd.print((int)peakPowerFwd);  
    lcd.print("W");  // Extra spaces to clear old characters
    lcd.setCursor(12, 0);
    lcd.print(vswr, 2);
    //lcd.print("  "); // Extra spaces to clear old characters
    bar(0, anF);
    lastUpdateLCD = millis();
  
}

///////////////// Reflected
void UpdateTXScreenREF() {
     // Peak Hold Logic
    if (pow_ref > peakPowerRef) {
      peakPowerRef = pow_ref;
      peakHoldTimer = millis();
    }

    if (millis() - peakHoldTimer > peakHoldTime) {
      peakPowerRef = pow_ref; // Update after hold time
    }

    lcd.setCursor(0, 0);
    lcd.print("Pr:0W");
    lcd.setCursor(3, 0);
    lcd.print((int)peakPowerRef);  
    lcd.print("W    ");  // Extra spaces to clear old characters
    lcd.setCursor(12, 0);
    lcd.print(vswr, 2);
    //lcd.print("  "); // Extra spaces to clear old characters
    bar(0, anR);
    lastUpdateLCD = millis();
  
}


void bar(int row, int lev) {
  // If level is 0, force smooth_lev and lmax to 0
  if (lev == 0) {
    smooth_lev[row] = 0;
    lmax[row] = 0;
  } else {
    // === Fast rise / slow decay logic ===
    if (lev > smooth_lev[row]) {
      smooth_lev[row] = lev;  // Fast rise
    } else {
      smooth_lev[row] = smooth_lev[row] + (lev - smooth_lev[row]) / Fall_time;  // Slow fall
    }

    // === Peak hold logic ===
    if (lev > lmax[row]) {
      lmax[row] = lev;
      dly[row] = -(T_PEAKHOLD) / T_REFRESH1;
    } else {
      if (dly[row] > 0)
        lmax[row] -= dly[row];

      if (lmax[row] < 0)
        lmax[row] = 0;
      else
        dly[row]++;
    }
  }

  lcd.setCursor(0, 1);
  lcd.write(' ');  // Filler space (optional)

  for (int i = 1; i < 16; i++) {
    int f = constrain(smooth_lev[row] - i * 5, 0, 5);
    int p = constrain(lmax[row] - i * 5, 0, 6);

    if (f)
      lcd.write(fill[f]);
    else
      lcd.write(peak[p]);
  }
}