// LM - TFT/Touchscreen interface for Hilltopper 40 - Requires Hilltopper CAT add-on code -
//      FFT code in this Teensy 3.5 sketch is based in part on the Norwegian Creations FFT example at -
//      https://www.norwegiancreations.com/2017/08/what-is-fft-and-how-can-you-implement-it-on-an-arduino/
//      The development name of this sketch is FFT-Timing_Test_TFT_touch_and_CAT.ino
//      The permanent saved name is TFT_touch_and_CAT_accessory_with_FFT_display_for_Hilltopper-40.ino
//      December 2019

#include "arduinoFFT.h"
#include <ILI9341_t3.h>
#include <UTouch.h>
 
// FFT
#define DATA_CHANNEL A1
#define NUM_SAMPLES 1024
#define SAMPLING_FREQUENCY 8192 // Hz
#define PLOT_FREQUENCY 4096     // Hz (Frequency to plot)
#define PLOT_SAMPLES NUM_SAMPLES*PLOT_FREQUENCY/SAMPLING_FREQUENCY

// Instantiate
arduinoFFT demoFFT = arduinoFFT();

// TFT
#define TFT_DC       9  // N/C
#define TFT_CS      10  // Display CS
#define TFT_RST    255  // 255 = unused, connect to 3.3V
#define TFT_MOSI    11
#define TFT_SCLK    13
#define TFT_MISO    12  // Not used (not connected)

//Touch
#define T_CS 38         // Touch CS
#define T_DO 29         // Touch data out (UTouch)
#define T_DIN 30        // Touch data in (UTouch)
#define T_CLK 36        // Touch clock (UTouch)
#define T_IRQ 22        // Touch IRQ

// Calibration kludge
#define X_PIXELS 320
#define Y_PIXELS 240
#define X_MIN 0         // Empirically determined minimum X returned by getX() for display part of screen
#define Y_MIN 26        // Same for Y
#define X_MAX 230       // Etc.     
#define Y_MAX 390 
#define X_RANGE (X_MAX - X_MIN)
#define Y_RANGE (Y_MAX - Y_MIN)

// Instantiate
ILI9341_t3 tft = ILI9341_t3(TFT_CS, TFT_DC, TFT_RST, TFT_MOSI, TFT_SCLK, TFT_MISO);
UTouch ts(T_CLK, T_CS, T_DIN, T_DO, T_IRQ);

#define X_PIXELS 320
#define Y_PIXELS 240

// Colors
#define  BLACK          0x0000
#define BLUE            0x001F
#define RED             0xF800
#define GREEN           0x07E0
#define CYAN            0x07FF
#define MAGENTA         0xF81F
#define YELLOW          0xFFE0  
#define WHITE           0xFFFF

// Color aliases
#define TFT_BG  YELLOW
#define TFT_TXT BLUE        // Text color 1
#define TFT_ALT RED         // Text color 2
#define TFT_ALT2 GREEN      // Text color 3
#define SLIDER_TEXT BLACK

// Additional colors (RGB)
const uint8_t LT_GREY[]  = {224, 224, 224};
const uint8_t LT_BEIGE[] = {255, 217, 102};

// Text displays related to received CAT data
const uint16_t DEFAULT_FONT_X_PIXELS = 5;           // At scale factor 1 (multiply by size)
const uint16_t DEFAULT_FONT_Y_PIXELS = 8;
const uint16_t DEFAULT_HOR_SEPARATION = 1;          // Characters are separated by 1 pixel (multiplied by size)
const uint16_t APPLICATION_VERT_MARGIN = 5;         // Amount of vertical clearance within which to register touch  
const uint16_t FREQ_SIZE = 3;
const uint16_t FREQ_X = 0;
const uint16_t FREQ_Y = 40;
const uint16_t FREQ_CHARACTER_COUNT = 15;
const uint16_t FREQ_CHARACTER_WIDTH = FREQ_SIZE * (DEFAULT_FONT_X_PIXELS + 1);
const uint16_t FREQ_CHARACTER_HEIGHT = FREQ_SIZE * DEFAULT_FONT_Y_PIXELS;
const uint16_t FREQ_DISPLAY_WIDTH = FREQ_CHARACTER_WIDTH * (FREQ_CHARACTER_COUNT - 5);
const uint16_t FREQ_DISPLAY_HEIGHT = FREQ_CHARACTER_HEIGHT; // Confine erasure to character height
const uint16_t FREQ_DISPLAY_TOUCH_Y = FREQ_Y - 2 * APPLICATION_VERT_MARGIN;
const uint16_t FREQ_DISPLAY_TOUCH_HEIGHT = FREQ_DISPLAY_HEIGHT + 2 * APPLICATION_VERT_MARGIN; 

const uint16_t STEP_X = FREQ_DISPLAY_WIDTH + FREQ_CHARACTER_WIDTH;
const uint16_t STEP_Y = FREQ_Y;
const uint16_t STEP_SIZE = 2;
const uint16_t STEP_CHARACTER_COUNT = 9;
const uint16_t STEP_CHARACTER_WIDTH = STEP_SIZE * (DEFAULT_FONT_X_PIXELS + 1);
const uint16_t STEP_CHARACTER_HEIGHT = STEP_SIZE * DEFAULT_FONT_Y_PIXELS;
const uint16_t STEP_DISPLAY_WIDTH = STEP_CHARACTER_WIDTH * STEP_CHARACTER_COUNT;
const uint16_t STEP_DISPLAY_HEIGHT = STEP_CHARACTER_HEIGHT + 2 * APPLICATION_VERT_MARGIN;

const uint16_t RIT_SIZE = 2;
const uint16_t RIT_X = 20;
const uint16_t RIT_Y = FREQ_Y + 30;
const uint16_t RIT_DN_UP_WIDTH = 80;
const uint16_t RIT_DN_UP_X = X_PIXELS - RIT_DN_UP_WIDTH - (RIT_X/2);
const uint16_t RIT_DN_UP_HEIGHT = 8 * RIT_SIZE + 4;

// For spectrum display
const uint16_t SP_HOR_MARGIN = 10;
const uint16_t SP_HOR_PIXELS = X_PIXELS - 2 * SP_HOR_MARGIN;
const uint16_t SP_VER_PIXELS = Y_PIXELS / 4;    // Height 1/4 of screen
const uint16_t SP_BASELINE = 100;               // Position below HT40 RIT frequency line
const uint16_t SP_BACKGROUND = YELLOW;
const uint16_t SP_FOREGROUND = BLUE;
const uint16_t SP_LEGEND_TEXT_SIZE = 1;
const uint16_t SP_LEGEND_TEXT_COLOR = RED;
const uint16_t SP_LEGEND_VER_DELTA = 5;
const uint16_t SP_LEGEND_LENGTH = 40;           // Computable... constant for convenience

const uint16_t SPD_SIZE = 1;
const uint16_t SPD_X = X_PIXELS - 70;
const uint16_t SPD_Y = Y_PIXELS - 10;

const uint16_t DEBUG_X = 10;
const uint16_t DEBUG_Y = SPD_Y; // Bottom line (shared with Morse speed display)
const uint16_t DEBUG_SIZE = 1;

const long FREQ_DELTA[FREQ_CHARACTER_COUNT] = {0, 0, 10000000, 1000000, 100000, 0, 10000, 1000, 100, 0, 0, 0, 0, 0, 0};
// const long FREQ_DELTA[FREQ_CHARACTER_COUNT] = {0, 0, 0, 10000000, 1000000, 100000, 0, 10000, 1000, 100, 0, 0, 0, 0, 0};

uint16_t lastX = -1;
uint16_t lastY = -1;
boolean touch = false;  // Track change when touched and released

// CAT
const byte EOC = 59;    // Semi-colon command terminator
char RxBuf[80];
char TxBuf[80];
int rIndex = 0;
int xIndex = 0;

// Application
#define catCom Serial1
const int TEST_LED = 37;                 // Development tool
const long TDELAY = 1000;                // Contextual touch release wait
const String ZERO = "0";
const unsigned long ONESEC = 1000L;
const unsigned long TWOSEC = 2000L;
const unsigned long TICKLE_WAIT = ONESEC / 2;
unsigned long lastTickle = 0;

const int BAND = 40;                     // For display only in this sketch
// Next needed for touch-screen initiated frequency change
const long  LOW_BAND_LIMIT =   700000000;
const long  HIGH_BAND_LIMIT =  730000000;

long int  OPfreq;
long int RITfreq;
String aFreq = "";                       // CAT format
String lastA = "";
String bFreq = "";
String lastB = "";
String fStep = "";
String lastS = "";
String mSpeed = "";
String lastM = "";
uint16_t main_screen_background_color;                  // Initialized to light grey in setup()

// For waterfall, where saturation denotes amplitude
const int SHADES = 231;
const uint16_t WF_VER_DELTA = 2;                // Test value
const uint16_t WF_BASELINE = SP_BASELINE + SP_VER_PIXELS + WF_VER_DELTA;
const uint16_t WF_HOR_MARGIN = 10;
const uint16_t WF_HOR_PIXELS = X_PIXELS - 2 * WF_HOR_MARGIN;
const uint16_t WF_VER_PIXELS = Y_PIXELS / 4;

// For both FFT displays
const int IGNORE_BINS = 2;                      // Presumed 'DC' effect from bins 0 through this bin

uint16_t spBackground;                          // Accommodate dynamic coloring
uint16_t spForeground;                          // Reserved

uint16_t wfColor[SHADES];
uint16_t wfScreen[WF_HOR_PIXELS][WF_VER_PIXELS];  // Screen copy
uint16_t packedAmplitudes[WF_HOR_PIXELS];

const unsigned long ONE_MILLION_MICROSECONDS = 1000000;
const double DOUBLE_ONE = 1.0;
const double SAMPLING_PERIOD_IN_MICROSECONDS = ONE_MILLION_MICROSECONDS*(DOUBLE_ONE/SAMPLING_FREQUENCY);

unsigned long sampling_period_in_microseconds;
unsigned long last_sample_time;
uint16_t auto_scaling_factor;

double vReal[NUM_SAMPLES];
double vImag[NUM_SAMPLES];


void setup() {
  pinMode(TEST_LED, OUTPUT);
  digitalWrite(TEST_LED, HIGH);
  // CAT
  catCom.begin(9600);
  catCom.flush();
  catCom.clear();
  // TFT
  main_screen_background_color = uColor(LT_GREY);
  tft.begin();
  tft.setRotation (1);
  // Touch
  ts.InitTouch(PORTRAIT);
  ts.setPrecision(PREC_MEDIUM);
  // FFT   
  sampling_period_in_microseconds = round(SAMPLING_PERIOD_IN_MICROSECONDS);
  // Spectrum display
  spBackground = uColor(255, 255, 204);
  // Waterfall
  // {230, 230, 255} is the lightest blue and {0, 0, 255} is the deepest blue,
  // where the range is 230 (+1) shades. 
  for (uint8_t i=0; i<SHADES; i++)
    wfColor[i] = uColor(i, i, SHADES-1);    // Shades of green would be wfColor[i] = uColor(i, SHADES-1, i);
  wfInit();
  // HT40 splash
  tftSplash();
  delay(TWOSEC);
  tft.fillScreen(main_screen_background_color);
  flashLED();
}

void loop() {
  double peakFrequency;
  // CAT
  catListen();
  tickle();
  tftUpdateDisplay();
  tftProcessTouch();
  // Acquire sample
  for(int i=0; i<NUM_SAMPLES; i++) {
    last_sample_time = micros();
    vReal[i] = analogRead(DATA_CHANNEL);    // Defaults to 10-bit resolution
    vImag[i] = 0;
    while (micros() < (last_sample_time + sampling_period_in_microseconds)) {
      tftProcessTouch();  // Check for touch
      ;                   // or simply wait
    }
  }

  // Compute FFT
  demoFFT.Windowing(vReal, NUM_SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  demoFFT.Compute(vReal, vImag, NUM_SAMPLES, FFT_FORWARD);
  demoFFT.ComplexToMagnitude(vReal, vImag, NUM_SAMPLES);
  peakFrequency = demoFFT.MajorPeak(vReal, NUM_SAMPLES, SAMPLING_FREQUENCY);

  // Process transformed data
  tftDisplayFFT();
  // tftPrintSpectrumLegend();       // Remove or comment-out when waterfall enabled (no room)
  tftRefreshWF();
  // delay(50);
  ;
}

// Adapted from Hilltopper_30band_LCD_RIT_CAT-restructure sketch
// CAT utilities

void clearRxBuf() {
  RxBuf[0] = (byte) 0;
  rIndex = 0;
}

void clearTxBuf() {
  TxBuf[0] = (byte) 0;
  xIndex = 0;
}

void clearCAT() {
  clearRxBuf();
  clearTxBuf();
}

void writeTxBuffer() {
  byte data_byte;
  xIndex = 0;
  while (true) {
    data_byte = (byte) TxBuf[xIndex++];
    if (data_byte == 0) break;
    catCom.write(data_byte);
    if (data_byte == EOC) break;
  }
  clearTxBuf();
}

void catListen() {
  byte data_byte;
  while (catCom.available()) {
    data_byte = catCom.read();
    if (data_byte > 31) {
      RxBuf[rIndex++] = (char) data_byte;
      if (data_byte == EOC) {
        processRxBuf();
      if ((byte)TxBuf[0] != 0)  // Not empty
        writeTxBuffer();
      lastTickle = millis();
      clearRxBuf();
      }
    }  
  }
}

void processRxBuf() {
  int rIndex = 0;
  if (RxBuf[0] == 'F' or RxBuf[0] == 'f') {
    rIndex++;
    if (RxBuf[1] == 'A' or RxBuf[1] == 'a') {
      rIndex++;
      if (RxBuf[rIndex] == (char)EOC) {
        // Process request for VFO-A frequency
      }
      else {
        aFreq = "";
        while (RxBuf[rIndex++] != EOC)
          aFreq.concat(RxBuf[rIndex-1]);
        // Set frequency to received value
      }
    } // FA
    if (RxBuf[1] == 'B' or RxBuf[1] == 'b') {
      rIndex++;
      if (RxBuf[rIndex] == (char)EOC) {
        // Process request for VFO-B frequency
      }
      else {
        bFreq = "";
        while (RxBuf[rIndex++] != EOC)
          bFreq.concat(RxBuf[rIndex-1]);
        // Set frequency to received value
      }
    } // FB
  }   // F-something
  else  if (RxBuf[0] == 'Z' or RxBuf[0] == 'z') {
    rIndex++;
    if (RxBuf[1] == 'S' or RxBuf[1] == 's') {
      rIndex++;
      fStep = "";
      while (RxBuf[rIndex++] != EOC)
        fStep.concat(RxBuf[rIndex-1]);
      // Ignore case HT asks for this application's step - Setting is one-way.
    }
    else if (RxBuf[1] == 'M' or RxBuf[1] == 'm') {
      rIndex++;
      mSpeed = "";
      while (RxBuf[rIndex++] != EOC)
        mSpeed.concat(RxBuf[rIndex-1]);
    }
  }   // Z-something
  
  clearRxBuf();
  rIndex = 0;
}

void sendFA() {
  // Inquire HT frequency
  clearTxBuf();
  TxBuf[0] = 'F';
  TxBuf[1] = 'A';
  TxBuf[2] = EOC;
  writeTxBuffer();
}

void sendFA(long freq) {
  // freq is in HT internal op format - convert to cat
  String s = "FA";
  s.concat(ht2cat(freq));
  s.concat((char)EOC);
  clearTxBuf();
  for (int i=0; i<s.length(); i++)
    TxBuf[i] = s.charAt(i);
  writeTxBuffer();
}

void sendFB() {
  // Inquire RIT frequency
  clearTxBuf();
  TxBuf[0] = 'F';
  TxBuf[1] = 'B';
  TxBuf[2] = EOC;
  writeTxBuffer();
}

void sendFB(long freq) {
  // freq is in HT internal op format - convert to cat
  String s = "FB";
  s.concat(ht2cat(freq));
  s.concat((char)EOC);
  clearTxBuf();
  for (int i=0; i<s.length(); i++)
    TxBuf[i] = s.charAt(i);
  writeTxBuffer();
}

void sendFB(String sFreq) {
  // sFreq is in CAT parameter format
  String s = "FB";
  s.concat(sFreq);
  s.concat((char)EOC);
  clearTxBuf();
  for (int i=0; i<s.length(); i++)
    TxBuf[i] = s.charAt(i);
  writeTxBuffer();
}

void sendZS() {
  // Inquire RIT frequency
  clearTxBuf();
  TxBuf[0] = 'Z';
  TxBuf[1] = 'S';
  TxBuf[2] = EOC;
  writeTxBuffer();
}

void sendZM() {
  // Inquire Morse speed
  clearTxBuf();
  TxBuf[0] = 'Z';
  TxBuf[1] = 'M';
  TxBuf[2] = EOC;
  writeTxBuffer();
}

void tickle() {
  // Periodically issue selected inquiries
  if (lastTickle + TICKLE_WAIT < millis()) {
    sendFA();
    sendFB();
    sendZS();
    sendZM();
    lastTickle = millis();
  }
}

boolean validCATfreq(String s) {
  // True if and only if String s encodes a CAT frequency FAnnnnnnnnnnn or FBnnnnnnnnnnn
  char c;
  for (int i=0; i<11; i++) {
    c = s.charAt(i);
    if ((byte) c < 48) return false;
    if ((byte) c > 57) return false;
  }
  return true;
}

String cat2tft(String s) {
    String sFreq;
    if (BAND == 40) {   // One digit shorter than 30 or 20 meter frequency
      sFreq = " ";
      sFreq.concat(s.charAt(4));
      sFreq.concat(".");
      sFreq.concat(s.substring(5,8));
      sFreq.concat("." );
      sFreq.concat(s.substring(8));
    }
    else {
      s.substring(3,5);
      sFreq.concat(".");
      sFreq.concat(s.substring(5,8));
      sFreq.concat("." );
      sFreq.concat(s.substring(8));
    }
    return sFreq.substring(0,10);    
}

String ht2cat(long freq) {
  freq /= 100;
  String s = "000";      // HT CAT uses Kenwood TS-480 version of the FA command.
  if (BAND == 40)
   s.concat(ZERO);
  return s.concat(String(freq));
}

// TFT and touch

// Display
void tftSplash() {
  tft.fillScreen(TFT_BG);
  tft.setTextColor(TFT_ALT);  
  tft.setTextSize(2);
  tft.setCursor(40, 80);
  tft.println("    Hilltopper " + String(BAND));
  tft.setCursor(35, 100);
  tft.println(" Touch screen version ");
  tft.setTextColor(TFT_TXT);  
  tft.setCursor(30, 140);
  tft.println("https://www.lloydm.net");
}

String tftFormatFrequency(long int freq) {
    String s = String(freq);
    String sFreq = "";
    if (BAND == 40)     // One digit shorter than 30 or 20 meter frequency
      sFreq = " ";
    sFreq.concat(s.substring(0,2) + "." + s.substring(2,5) + "." + s.substring(5,8));

    return sFreq;  
}
void tftDisplayMainFrequency(String sFreq) {  // VFO A
  // Clear main frequency
  tft.fillRect(FREQ_X, FREQ_Y, FREQ_DISPLAY_WIDTH, FREQ_DISPLAY_HEIGHT, main_screen_background_color);
  tft.setTextColor(TFT_TXT);
  tft.setTextSize(FREQ_SIZE);
  tft.setCursor(FREQ_X, FREQ_Y);
  tft.println(sFreq);
}

void tftDisplayAuxFrequency(String sFreq) {   // VFO B
  // RIT adjustment box
  String bFreq = "RIT:";
  if (BAND != 40)                             // cat2tft() returns a leading space when BAND == 40
    bFreq.concat(" ");
  bFreq.concat(sFreq);
  //  Clear RIT
  tft.fillRect(0, RIT_Y, RIT_DN_UP_X-5, RIT_DN_UP_HEIGHT, main_screen_background_color);
  tft.setTextColor(TFT_ALT);
  tft.setTextSize(RIT_SIZE);
  //  Update VFO-B display
  tft.setCursor(RIT_X, RIT_Y);
  tft.println(bFreq);
  tft.setCursor(RIT_DN_UP_X, RIT_Y);
//tft.drawRect(RIT_DN_UP_X-5, RIT_Y-3, RIT_DN_UP_WIDTH, RIT_DN_UP_HEIGHT, BLACK);
  tft.fillRect(RIT_DN_UP_X-5, RIT_Y-3, RIT_DN_UP_WIDTH, RIT_DN_UP_HEIGHT, uColor(LT_BEIGE));
  tft.drawFastVLine(RIT_DN_UP_X+(RIT_DN_UP_WIDTH/2)-5, RIT_Y-3, RIT_DN_UP_HEIGHT, BLACK);
  tft.println("DN  UP");
  tft.drawRect(RIT_DN_UP_X-5, RIT_Y-3, RIT_DN_UP_WIDTH, RIT_DN_UP_HEIGHT, BLACK);
}

void tftDisplayStep(String fStep) {
  String s = "Step: ";
  if (fStep.charAt(0) == ' ')
    s.concat(fStep.substring(1));
  else
    s.concat(fStep);
  tft.fillRect(STEP_X, STEP_Y, STEP_DISPLAY_WIDTH, STEP_DISPLAY_HEIGHT, main_screen_background_color);
  tft.setTextColor(BLACK);
  tft.setTextSize(STEP_SIZE);
  tft.setCursor(STEP_X,STEP_Y);
  tft.println(s);
}

void tftDisplayMorseSpeed() {
  // Append (Do not clear screen)
  String sWPM = "[" + mSpeed + " wpm" + "]";   // Current speed setting
  tft.fillRect(SPD_X, SPD_Y, X_PIXELS-SPD_X, Y_PIXELS-SPD_Y, main_screen_background_color);
  tft.setTextColor(TFT_ALT);
  tft.setTextSize(SPD_SIZE);
  tft.setCursor(SPD_X, SPD_Y);
  tft.println(sWPM);
}

void tftUpdateDisplay() {
  // On startup, while HT is annunciating frequency in Morse, FA returns '00000'
  if (validCATfreq(aFreq)) {              // Don't update VFO-A or VFO-B until startup is complete
    // Update VFO-A frequency display
    if (aFreq.compareTo(lastA) != 0){     // VFO-A changed
      tftDisplayMainFrequency(cat2tft(aFreq));
      lastA = aFreq;
    }
    if (bFreq.compareTo(lastB) != 0) {    // VFO-B changed
      if (validCATfreq(bFreq)) {
        // Update VFO-B frequency display
        tftDisplayAuxFrequency(cat2tft(bFreq));
        lastB = bFreq;
      }
   }
  }
  if (fStep.compareTo(lastS) != 0) {
    tftDisplayStep(fStep);
    lastS = fStep;
  }
  if (mSpeed.compareTo(lastM) != 0) {
    tftDisplayMorseSpeed();
    lastM = mSpeed;
  }
}

// Touch
int myGetX() {
  return (ts.getX() - X_MIN) * X_PIXELS / X_RANGE;
}

int myGetY() {
  return (ts.getY() - Y_MIN) * Y_PIXELS / Y_RANGE;
}

boolean tftTouchedInBox(uint16_t x, uint16_t y, uint16_t w, uint16_t h) {
  if (lastX < x) return false;
  if (lastY < y) return false;
  if (lastX > (x + w)) return false;
  if (lastY > (y + h)) return false;
  return true;
}

void tftProcessTouch() {
  if (!ts.dataAvailable()) {
    touch = false;
    return;
  }
  tftCompleteTouchProcessing();
  return;
}

void tftCompleteTouchProcessing() {
  // This function is called by tftProcessTouch
  // Following constants reflect empirical calibration of frequency touch,
  // and substitute for similar global constants
  const int PIXELS_PER_CHARACTER = 20;
  const int NUM_OF_CHARACTERS = 10;
  //
  String tmp = ""; // DEBUG
  ts.read();
  int x = myGetX();
  int y = myGetY();
  if (touch || (x < 0) || (y < 0)) {
    return;
  }
  touch = true;
  lastX = x;
  lastY = y;
  boolean up, down;
  if (tftTouchedInBox(FREQ_X, FREQ_DISPLAY_TOUCH_Y, FREQ_DISPLAY_WIDTH, FREQ_DISPLAY_TOUCH_HEIGHT)) {
    // Touched in main frequency box
    OPfreq = aFreq.toInt() * 100;   // toInt() returns long!
    // See above note (Experimental)
    int iSub = (lastX - FREQ_X)/ PIXELS_PER_CHARACTER;
    if (iSub >= NUM_OF_CHARACTERS)
      iSub = NUM_OF_CHARACTERS;
    // Up/down dividing line denominator '4' was experimentally determined
    up = lastY < (FREQ_Y + FREQ_CHARACTER_HEIGHT/4);
    down = lastY > (FREQ_Y + FREQ_CHARACTER_HEIGHT/4);
    if (up) {
      OPfreq = OPfreq + FREQ_DELTA[iSub];
      // Band limit check
      if (OPfreq > HIGH_BAND_LIMIT)
        OPfreq = HIGH_BAND_LIMIT;
    }
    else if (down) {
      OPfreq = OPfreq - FREQ_DELTA[iSub];
     // Band limit check
      if (OPfreq < LOW_BAND_LIMIT)
        OPfreq = LOW_BAND_LIMIT;
    }
    sendFA(OPfreq); 
  }
    else if (tftTouchedInBox(RIT_DN_UP_X-5, RIT_Y-3, RIT_DN_UP_WIDTH, RIT_DN_UP_HEIGHT)) {
    // RIT up/down
    up = RIT_DN_UP_X+(RIT_DN_UP_WIDTH/2) < lastX;
    down = RIT_DN_UP_X+(RIT_DN_UP_WIDTH/2) > lastX;
    if (up) {
      // Increment RIT by step
      sendFB(sAdd(lastB, fStep.toInt()));
    }
    if (down) {
      // Decrement RIT by step
      sendFB(sAdd(lastB, -fStep.toInt()));
    }
  }
}


// Spectrum and waterfall displays

void wfInit() {
  for (int i=0; i<WF_HOR_PIXELS; i++)
    for (int j=0; j<WF_VER_PIXELS; j++) {
      wfScreen[i][j] = SHADES-1;  
    }
}

void wfScroll() { // DEBUG
  for (int i=0; i<WF_HOR_PIXELS; i++) {
    for (int j=WF_VER_PIXELS-1; j>0; j--)
      wfScreen[i][j] = wfScreen[i][j-1];  
  }
}

void tftRefreshWF() {
  // Assume that packedAmplitudes array has been computed (e.g. for spectrum display)
  int y;
  wfScroll();
  for (int i=0; i<WF_HOR_PIXELS; i++) {
    // y = packedAmplitudes[i] * (SHADES - 1) / auto_scaling_factor;
    y = SHADES - (packedAmplitudes[i] * SHADES / auto_scaling_factor);
    if (y > (SHADES - 1))
      y = SHADES - 1;
    wfScreen[i][0] = y;
  }
  tftCopyArrayToScreen();
}

void tftCopyArrayToScreen() {
  for (int i=0; i<WF_HOR_PIXELS; i++)
    for (int j=0; j<WF_VER_PIXELS; j++) {
      tft.drawPixel(WF_HOR_MARGIN + i, WF_BASELINE + j, wfColor[wfScreen[i][j]]);
    }
  //tftDrawWaterfallBorder();
}

void tftDrawWaterfallBorder() {
  tft.drawRect(WF_HOR_MARGIN-1, WF_BASELINE-1, WF_HOR_PIXELS+2, WF_VER_PIXELS+2, BLACK);
}

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

void tftDisplayFFT() {
  // Invoke test display functions from here
  int y;
  computePackedAmplitudesArray();
  for (int i=0; i<SP_HOR_PIXELS; i++) {
    y = packedAmplitudes[i] * SP_VER_PIXELS / auto_scaling_factor;
    if (y > SP_VER_PIXELS)
      y = SP_VER_PIXELS;
    tftPlotSpPoint(i, y);
  }
  tftDrawSpectrumPlotBorder();
}

void tftDrawSpectrumPlotBorder() {
  tft.drawRect(SP_HOR_MARGIN, SP_BASELINE, SP_HOR_PIXELS, SP_VER_PIXELS, BLACK);
  // Add [optional] tick marks
}

void tftPrintSpectrumLegend() {
  int fKhz = PLOT_FREQUENCY / 1000;
  tft.setTextSize(SP_LEGEND_TEXT_SIZE);
  tft.setTextColor(SP_LEGEND_TEXT_COLOR);
  tft.setCursor(SP_HOR_MARGIN, SP_BASELINE+SP_VER_PIXELS+SP_LEGEND_VER_DELTA);
  tft.print("0");
  tft.setCursor((X_PIXELS-SP_LEGEND_LENGTH)/2, SP_BASELINE+SP_VER_PIXELS+SP_LEGEND_VER_DELTA);
  tft.print(String(fKhz/2) + " KHz");
  tft.setCursor(X_PIXELS-SP_HOR_MARGIN-SP_LEGEND_LENGTH, SP_BASELINE+SP_VER_PIXELS+SP_LEGEND_VER_DELTA);
  tft.print(String(fKhz) + " KHz");  
}

void tftPlotSpPoint(uint16_t x, uint16_t y) {
  tft.drawFastVLine(x + WF_HOR_MARGIN, SP_BASELINE, SP_VER_PIXELS, spBackground);
  tft.drawFastVLine(x + WF_HOR_MARGIN, SP_BASELINE + SP_VER_PIXELS - y, y, SP_FOREGROUND);
}

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

void computePackedAmplitudesArray() {
  // Assumes number of bins >= number of horizontal pixels
  int bin, lastBin = 0;
  int nPerBin = 0;
  int binSum = 0;
  boolean newBin;
  uint16_t minBin = 0xFFFF;
  uint16_t maxBin = 0;
  for (int i=0; i<(PLOT_SAMPLES); i++) {
    bin = round(i * 1.0 * WF_HOR_PIXELS / (PLOT_SAMPLES));
    newBin = (bin != lastBin);
    if (newBin) {
      packedAmplitudes[lastBin] = binSum / nPerBin;
      if (bin > IGNORE_BINS) {   // Bins 0, 1, 2 include large DC amplitude
      if (maxBin < packedAmplitudes[lastBin])
        maxBin = packedAmplitudes[lastBin];
      if (minBin > packedAmplitudes[lastBin])
        minBin = packedAmplitudes[lastBin];
      }
      binSum = 0;
      nPerBin = 0;
      lastBin = bin;
    }
    binSum += vReal[i];
    nPerBin++;
  }
  auto_scaling_factor = maxBin - minBin + 1;
  packedAmplitudes[0] = 0;       // Bin 0 value is spurious (DC) - Set to 0.
}



// Adapted from: https://afterthoughtsoftware.com/posts/convert-rgb888-to-rgb565

uint16_t uColor(uint8_t RGB[]) {
  
    uint16_t b = (RGB[2] >> 3) & 0x1f;
    uint16_t g = ((RGB[1] >> 2) & 0x3f) << 5;
    uint16_t r = ((RGB[0] >> 3) & 0x1f) << 11;

    return (uint16_t) (r | g | b); 
}


uint16_t uColor(uint8_t R, uint8_t G, uint8_t B) {
  
    uint16_t b = (B >> 3) & 0x1f;
    uint16_t g = ((G >> 2) & 0x3f) << 5;
    uint16_t r = ((R >> 3) & 0x1f) << 11;

    return (uint16_t) (r | g | b);
}

// Miscellaneous 

String sAdd(String a, long bNum) {
  // Add CAT frequency parameter 'a' and step 'b'
  // Return in frequency parameter format
  long aNum = a.toInt();
  String s = String(aNum + bNum);
  String t = "";
  for (int i=s.length(); i < a.length(); i++)
    t.concat("0");
  return t.concat(s);
}

void tftDisplayDebugData(String sInfo) {
  tft.fillRect(DEBUG_X, DEBUG_Y, SPD_X-DEBUG_X, 40, main_screen_background_color);
  tft.setTextColor(TFT_ALT);
  tft.setTextSize(DEBUG_SIZE);
  tft.setCursor(DEBUG_X, DEBUG_Y);
  tft.println(sInfo);
}

void flashLED() {
    digitalWrite(TEST_LED, LOW);
    delay(ONESEC/8);
    digitalWrite(TEST_LED, HIGH);
    delay(ONESEC/8);
    return;  
}

void flashLED(int n) {
  for (int i=0; i<n; i++)
    flashLED();
  return;  
}