///////////////////////////////////////////////////////
//                                                   //
//     ATU FOR END FED HALF WAVE ANTENNA 20 & 40m    //
//                   by ON7EQ                        //
//                                                   //
//         full tune sequence by CAT control         //
//                                                   //
///////////////////////////////////////////////////////


#include <Servo.h>   // We will use a servo
Servo servo;           // for rotating the capacitor

#include <NewSoftSerial.h>
// Use pins 2 and 3 to talk to the CAT. 2 is the RX pin, 3 is the TX pin 
// Connect the RX pin to the CAT output through a 4k7 resistor.
NewSoftSerial CATserial =  NewSoftSerial(2, 3);

#include <LiquidCrystal.h>
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

//// DEFINE FIXED DATA /////

#define TrxAddress (102)          // 102 = HEX66 = IC746Pro / 7400
#define LowPowerSet (8)          // the DEC figures sets the low power output level (max = 55 !) 25 = 5w about 5 = 2w about
#define servoControlPin (4)      // attached to pin 4
#define servoPowerPin (A5)        // attached to pin A5: H = power to servo
#define backLash (10)              //servo backlash
#define TuneRequestPin (5)        // attached to pin 5 : L = Tune request by Transceiver
#define TuneTxPin (13)            // attached to pin 13 : L = setr TRX in Tune mode
#define BuzzerPin (A0)            // attached to Analog pin 0 : H = Buzzer beep
#define manualTunePin (A1)         // toggles switch attached to Analog pin : 0 = down, +5v = up; 2.5 v = idle
#define VccMeasurePin (A4)        // attached to Analog pin 4 : Voltage measure (1k2 to GND, 4k7 to Vcc - max 25v)
#define SwrFwdPin (A2)             // attached to Analog pin 2 : SWR Forward probe  (max 5v)
#define SwrReflPin (A3)            // attached to Analog pin 3 : SWR Reflected probe  (max 5v)
#define relayPin (6)               // attached to Digital pin 6 : Relay control - High = 40m select
#define R1           (12)   // from GND to VccMeasurePin, express in 100R  (12 = 1200 Ohm)
#define R2           (47)   // from + power supply to VccMeasurePin, express in 100R  (47 = 4700 Ohm)



/// SOME VARIABLES

int i;
int incoming;
int buffget[10] ;                             // the receive buffer

int Swr_fwd = 0;
int Swr_refl = 0;

unsigned int SupplyVoltage = (0); // Power supply voltage

byte capPos = 255;
byte bestCapPos = 0;
byte HighCapPos = 0;
byte LowCapPos = 0;

byte refl = 0;
byte bestRefl = 255;

byte ActMode = 0;                             // Actual mode                    
byte ActPwr_msb = 0;                            // Actual power msb
byte ActPwr_lsb = 0;                            // Actual power lsb

byte  TuneRequestPin_status = (1);                                // High level is idle
long  TuneRequestPinbuttonTime = 0;
long  lastCat = 0;

int unsigned long MHZ = 0;
int unsigned long KHZ = 0;
int unsigned long HZ = 0;
int unsigned long QRG = 0; 
byte band = 0;



// read commands
byte swr_read[7] = {254,254,TrxAddress,224,21,18,253};           // SWR read command
byte read_trx_id[6] = {254,254,TrxAddress,224,25,253};           // read TRX ID, irrespective if actal address
byte mode_read[6] = {254,254,TrxAddress,224,04,253};             // read mode: response $FE $FE $EO ra $04 $03 $02 $FD // $03 = CW
byte rf_power_read[7] = {254,254,TrxAddress,224,20,10,253};      // read RF powermode: response $FE $FE ra $E0 $14 $10 $yy $yy $FD 
byte qrg_read[6] = {254,254,TrxAddress,224,03,253};              // read frequency 


//write commands
byte dial_lock[8] = {254,254,TrxAddress,224,22,80,01,253};       // dial lock
byte dial_unlock[8] = {254,254,TrxAddress,224,22,80,00,253};     // dial unlock
byte low_power[9] = {254,254,TrxAddress,224,20,10,00,LowPowerSet,253};    // low power
byte tx[8] = {254,254,TrxAddress,224,28,00,01,253};             //turn Tx on
byte rx[8] = {254,254,TrxAddress,224,28,00,00,253};             // goto receive

byte mode_fm[7] = {254,254,TrxAddress,224,6,5,253};              // FM mode
byte mode_rtty[7] = {254,254,TrxAddress,224,6,4,253};            // RTTY mode

byte mode_return[5] = {254,254,TrxAddress,224,6};                // 1st 5 bytes, need to be completed
byte rf_power_set[6] = {254,254,TrxAddress,224,20,10};           // 1st 6 bytes, need to be completed with $yy $yy $FD 

byte degree [8] = {  // special character
  B00100,
  B01010,
  B00100,
  B00000,
  B00000,
  B00000,
  B00000,
};


 ////////////////////   S E T U P  //////////////////////////

void setup() {

  //Serial.begin(57600);   // debug
  
  CATserial.begin(9600);  
   // set up the LCD's number of columns and rows: 
  lcd.begin(8, 2);
  lcd.clear(); 

// Print a message to the LCD.
lcd.print(" ON7EQ");
lcd.setCursor(0, 1);
lcd.print("Icom ATU");

delay (3000);
lcd.clear();  

// create special characters  
lcd.createChar(0, degree);


pinMode(TuneRequestPin, INPUT);

pinMode(BuzzerPin, OUTPUT);
digitalWrite(BuzzerPin, LOW);

pinMode(servoPowerPin, OUTPUT);  
digitalWrite(servoPowerPin, LOW);

digitalWrite(TuneTxPin, LOW);
pinMode(TuneTxPin, OUTPUT); 

pinMode(relayPin, OUTPUT);

readFrequency();

TuneRequestPinbuttonTime = millis();

}
 
////////////  L   O   O  P  /////////////// 
 
void loop() {


  waitbutton:

      if ((analogRead(manualTunePin) < 340) and (band != 0)) manualDown(); // manual tuning requested
      if ((analogRead(manualTunePin) > 680) and (band != 0)) manualUp();
      
      readFrequency();
       
      TuneRequestPin_status = digitalRead(TuneRequestPin);
      if (TuneRequestPin_status == 1) {
      TuneRequestPinbuttonTime = millis();
      goto waitbutton;
       }
  if ((millis() - TuneRequestPinbuttonTime) < 50)  goto waitbutton;
  
  // button is pushed !
   TuneRequestPin_status = 1;    //Reset button
   if (band == 0) {      // Band error !
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);             
                delay(60);
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);
                delay(60);
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);             
                delay(60);
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);      
     delay(500);
    goto waitbutton;
    }
   
   lcd.setCursor(0, 1); 
   lcd.print("-TUNING-"); 

    delay(500);                  // wait till tune button released / DO NOT REMOVE ! 

enterTuneMode();
//   delay(5000);
//  lcd.clear();   

  servo.attach(servoControlPin,575,2650);

  bestCapPos = 0;
  bestRefl = 1023;
  digitalWrite(servoPowerPin, HIGH);
  delay(200); 
  capPos = 0;
  servo.write(0);      // turn capacitor to start position
  delay(600);             // this will take a bit of time, so wait

  ///// find best reflection  ///// 
  
  /// coarse
  
  for(capPos = 0; capPos < 181; capPos += 10) {
  
      servo.write(capPos);
      lcd.setCursor(0, 0);
      if (capPos < 100)  lcd.print("0");
      if (capPos < 10)  lcd.print("0");
      lcd.print(capPos, DEC); 
      lcd.write(0);
 

//lcd.setCursor(0, 0);
 
      delay(150);   // Wait for servo 

 
 
     Swr_refl = analogRead(SwrReflPin);
//   Swr_fwd = analogRead(SwrFwdPin);
   
   refl = Swr_refl;
   if (refl > 999) refl = 999;
 
 // end SWR by probe input
 
    lcd.print(" "); 
    if (refl < 100)  lcd.print("0");
    if (refl < 10)  lcd.print("0");
    lcd.print(refl, DEC); 
 
    delay (20); 
    if (refl < bestRefl) {
      bestRefl = refl;
      bestCapPos = capPos;
      }
  }



 /// fine
 
   HighCapPos = bestCapPos + 20;
   LowCapPos = bestCapPos - 20;
   bestRefl = 1023;
  servo.write(LowCapPos);      // turn capacitor to start position
  delay(400);                       // this will take a bit of time, so wait
  
  for(capPos = LowCapPos; capPos < (HighCapPos + 1); capPos += 1) {
  
      servo.write(capPos);
      lcd.setCursor(0, 0);
      if (capPos < 100)  lcd.print("0");
      if (capPos < 10)  lcd.print("0");
      lcd.print(capPos, DEC); 
      lcd.write(0);
 

//lcd.setCursor(0, 0);
 
      delay(200);   // Wait for servo & vswr_refl voltage decay
 
     Swr_refl = analogRead(SwrReflPin);
//   Swr_fwd = analogRead(SwrFwdPin);
   
   refl = Swr_refl;
   if (refl > 999) refl = 999;
 
 // end SWR by probe input
 
    lcd.print(" "); 
    if (refl < 100)  lcd.print("0");
    if (refl < 10)  lcd.print("0");
    lcd.print(refl, DEC); 
 
    delay (50); 
    if (refl < bestRefl) {
      bestRefl = refl;
      bestCapPos = capPos;
      // if (refl > bestRefl) goto endTuneProcess ; 
      }
  }

  // select best capacitance 

 endTuneProcess:
 
 capPos = capPos --;
 servo.write(capPos);
     delay (100); 
 if (capPos > (bestCapPos - backLash)) goto endTuneProcess;
 

      lcd.setCursor(0, 0);
      if (bestCapPos < 100)  lcd.print("0");
      if (bestCapPos < 10)  lcd.print("0");
      lcd.print(bestCapPos, DEC); 
      lcd.write(0);

      
       lcd.print(" "); 
      if (bestRefl < 100)  lcd.print("0");
      if (bestRefl < 10)  lcd.print("0");
      lcd.print(bestRefl, DEC); 

  // end tuning process
 
 
  
    lcd.setCursor(0, 1); 
     lcd.print(" READY!  ");
    delay(500);
    servo.detach(); 
    digitalWrite(servoPowerPin, LOW);
    digitalWrite(BuzzerPin, HIGH);  
            delay(150);
    digitalWrite(BuzzerPin, LOW); 
    delay(200);
    
exitTuneMode();
      
  }
  
/////////////////////////////////////////////////////////////////////////////////////////////////////////
  
void enterTuneMode() {  
// readFrequency();     // detect frequency, select relay
    delay(200); 
    
 while(CATserial.available()) CATserial.read();     // Flush buffer
    delay(20);


  for( i=0; i<8; i++){          // send DIAL LOCK  --> reply
    CATserial.print(dial_lock[i],BYTE); }
    delay(50);




////// retrieve actual mode & power /////

// send mode  request 
 
 mode_request:

 while (CATserial.available() > 0) CATserial.read(); // clear buffer
 delay(10); 
 for( i=0; i<6; i++){          
    CATserial.print(mode_read[i],BYTE); }
   
    delay(50); 
    
// read reply - format should be $FE $FE $EO ra $04 $03 $02 $FD -- $03 for CW

  m_next:
    if (CATserial.available() > 0) {
    incoming = CATserial.read();
    if (incoming == 254) goto m_start;            // 1st byte is  an FE look for an FE to start
       goto m_next; }
  m_start:
    buffget[0] = 0; 
    delay(1);
    for ( i=0;i<7;i++) {                   // get next 7 bytes
    if(CATserial.available() >0) {
    buffget[i]=CATserial.read();           // load buffget with next 8 bytes
    delay(2);                             //delay 2 ms if true, time for buffer fill
        }
    }                                     // OK, got new 7 byte buffget array

    delay (10);                            // Required delay to process buffer, do not delete 

    if ((buffget[0] == 254) and (buffget[3] == 4) and (buffget[6] == 253))  goto mode;  // again FE , and mode byte  and last byte FD?
    goto m_next;                         // wrong array, get another
 
 mode:                                 // good array on hand, have a mode readout !
  ActMode = buffget[4]; 
  
  
// send rf power level 
 
 pwr_request:

 while (CATserial.available() > 0) CATserial.read(); // clear buffer
 delay (20); 
 for( i=0; i<7; i++){          
    CATserial.print(rf_power_read[i],BYTE); }
   
    delay(50); 
  
// read reply - format should be $FE $FE ra $E0 $14 $10 $yy $yy $FD

  pwr_next:
    if (CATserial.available() > 0) {
    incoming = CATserial.read();
    if (incoming == 254) goto pwr_start;            // 1st byte is  an FE look for an FE to start
       goto pwr_next; }
  pwr_start:
    buffget[0] = 0; 
    delay(1);
    for ( i=0;i<8;i++) {                   // get next 8 bytes
    if(CATserial.available() >0) {
    buffget[i]=CATserial.read();           // load buffget with next 8 bytes
    delay(2);                             //delay 2 ms if true, time for buffer fill
        }
    }                                     // OK, got new 8 byte buffget array

    delay (10);                          // Required delay to process buffer, do not delete 

    if ((buffget[0] == 254) and (buffget[3] == 20) and (buffget[4] == 10)and (buffget[7] == 253))  goto read_power;  // again FE , and mode byte  and last byte FD?
    goto pwr_next;                       // wrong array, get another
 
 read_power:                               // good array on hand, have a power level readout !
  ActPwr_msb = buffget[5]; 
  ActPwr_lsb = buffget[6]; 
 
    
  //end send mode  request    

//   
  for( i=0; i<7; i++){          // send mode RTTY cmd
    CATserial.print(mode_rtty[i],BYTE); }
    delay(50); 
  for( i=0; i<9; i++){          // send LOW POWER 
    CATserial.print(low_power[i],BYTE); }
    delay(50); 
  for( i=0; i<8; i++){          // send TX  
    CATserial.print(tx[i],BYTE); }
    delay(50); 
  }
  
/////////////////////////////////////////////////////////////////////////////////////////////////////////

void exitTuneMode() { 
 
 while (CATserial.available() > 0) CATserial.read(); // clear buffer
 delay (200);
 
  for( i=0; i<8; i++){          // sendRX 
    CATserial.print(rx[i],BYTE); }
    delay(50); 


// set again mode 
   for( i=0; i<5; i++){          
    CATserial.print(mode_return[i],BYTE); }
    CATserial.print(ActMode,BYTE);
    CATserial.print(253,BYTE); 
    delay(50);
 
// set again rf power level 
   for( i=0; i<6; i++){          
    CATserial.print(rf_power_set[i],BYTE); }
    CATserial.print(ActPwr_msb,BYTE);
    CATserial.print(ActPwr_lsb,BYTE);
    CATserial.print(253,BYTE); 
    delay(50);

  for( i=0; i<8; i++){          // send DIAL UNLOCK 
    CATserial.print(dial_unlock[i],BYTE); }
    delay(50);      
 }  
   
/////////////////////////////////////////////////////////////////////////////////////////////////////////

void readFrequency() { 
 
   while (CATserial.available() > 0) CATserial.read(); // clear buffer
   delay (20);

   
 new_try:
    lastCat = millis();
    for( i=0; i<6; i++){          // send qrg read
    CATserial.print(qrg_read[i],BYTE); }
    delay(50);  
   
   
  qrg_next:
  
  if ((millis() - lastCat) > 300)  goto new_try;
  
  if (CATserial.available() > 0) {
    lastCat = millis();
    incoming = CATserial.read();
    if (incoming == 254) {           // 1st byte is  an FE look for an FE to start
      goto qrg_start; 
    }
     goto qrg_next; 
  }

qrg_start:

  buffget[0] = 0; 

  // delay(1);
  for ( i=0;i<10;i++) {               // get next 10 bytes
    if(CATserial.available() > 0) {
      buffget[i]=CATserial.read();     // load buffget with next 10 characters
      delay(2);                       //delay 1 ms if true, time for buffer fill
    }
  } 
  


delay (10);                                     // delay is required to process buffer, do not remove !

  if (buffget[0] == 254) {                      // again FE , as 2nd character ?

    goto qrg_next1; 
  }                                             // yes, now do '00' test
  goto qrg_next;                                // wrong array, get another


qrg_next1:

  if ((buffget[3] == 0) or (buffget[3] == 3) or  (buffget[3] == 5)     ){  // check to see if 4th char is 00 or 03 or 05 (=frequency)
    goto qrg_next2; 
  }                                             // detected, goto next2

  goto qrg_next;                                  // wrong array, get another


qrg_next2:

// Check for last byte

  if(buffget[9] == 253 ){                       // look for FD at end of array
    goto frequency; 
  }                                             // if FD detected, goto frequency
  goto qrg_next;                                // wrong command



frequency:                                     // we have frequency array on hand

    MHZ = (buffget[7]);

    MHZ = MHZ - (((MHZ/16) * 6));              // Transform bytes ICOM CAT 
    if (MHZ >= 100)  goto qrg_next;              // wrong byte

    KHZ = buffget[6];
    KHZ = KHZ - (((KHZ/16) * 6));              // Transform bytes ICOM CAT 
    if (KHZ >= 100)  goto qrg_next;              // wrong byte

    HZ = buffget[5]; 
    HZ = HZ - (((HZ/16) * 6));                 // Transform bytes ICOM CAT 
    if (HZ >= 100)  goto qrg_next;               // wrong byte

  QRG = ((MHZ * 10000) + (KHZ * 100) + (HZ * 1)); // QRG variable stores frequency in MMkkkH  format

  band  = 0 ;  // Reset band info
  if ((QRG > 139000) and (QRG < 145000)) {
       band = 20;
       digitalWrite(relayPin, LOW);  
     }
  if ((QRG > 69900) and (QRG < 72100)) {
       band = 40;
       digitalWrite(relayPin, HIGH);  
     }
     //debug
     lcd.setCursor(0, 1); 
     if ((band == 20) or (band == 40)) {
     lcd.print(band,DEC);
     lcd.print("m ");
     }
     if (band == 0) lcd.print("ERR "); 
     
  SupplyVoltage = analogRead(VccMeasurePin);        // Read power supply voltage
  SupplyVoltage = map(SupplyVoltage, 0,1023,0,(50*(R2+R1)/R1));
              if (SupplyVoltage < 100) {
                 lcd.print(" ");
                      } 
                 if (SupplyVoltage < 10) {
                      lcd.print(" ");
                      } 
                 lcd.print((SupplyVoltage/10), DEC);
                     lcd.print("v");
                 lcd.print((SupplyVoltage)%10, DEC); 
   }   
  
///////////////////   M A N U A L   T U N E /////////////////////

void manualUp() { 

   lcd.setCursor(0, 1); 
   lcd.print("+MANUAL+");
 
  servo.attach(servoControlPin,575,2650);
  if (capPos < 180) digitalWrite(servoPowerPin, HIGH);
  delay(200); 
  if (capPos  == 255) {
      digitalWrite(servoPowerPin, HIGH);
      capPos = 0;
      servo.write(0);      // turn capacitor to start position if cappos never updated
      delay(800);          // this will take a bit of time, so wait
      } 
  
  digitalWrite(TuneTxPin, HIGH); 

  againPlus:
  
  capPos = capPos ++;
  if (capPos == 181) {
      capPos = 180;
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);             
                delay(60);
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);    
      goto endPlus;
      }  
 
  servo.write(capPos);
  
  lcd.setCursor(0, 0);
  if (capPos < 100)  lcd.print("0");
  if (capPos < 10)  lcd.print("0");
  lcd.print(capPos, DEC); 
  lcd.write(0);
    
  Swr_refl = analogRead(SwrReflPin);
  refl = Swr_refl;
  if (refl > 999) refl = 999;
  lcd.print(" "); 
  if (refl < 100)  lcd.print("0");
  if (refl < 10)  lcd.print("0");
  lcd.print(refl, DEC); 
  
  delay(100);  
  if (analogRead(manualTunePin) > 680) goto againPlus; 

  endPlus:

  digitalWrite(TuneTxPin, LOW); 

  lcd.setCursor(0, 1); 
  lcd.print("  STBY  ");

  servo.detach(); 
  digitalWrite(servoPowerPin, LOW);
  delay(500);
 }  
 
 void manualDown() { 
  
   lcd.setCursor(0, 1); 
   lcd.print("-MANUAL-");
  
  servo.attach(servoControlPin,575,2650);
  if (capPos > 0)digitalWrite(servoPowerPin, HIGH);
  delay(200); 
  if (capPos  == 255) {
      capPos = 0;
      servo.write(0);      // turn capacitor to start position if cappos never updated
      delay(800);          // this will take a bit of time, so wait
      } 
  digitalWrite(TuneTxPin, HIGH); 

  
  againDown:
  
  if (capPos > 0)capPos = capPos --;
  
  lcd.setCursor(0, 0);
  if (capPos < 100)  lcd.print("0");
  if (capPos < 10)  lcd.print("0");
  lcd.print(capPos, DEC); 
  lcd.write(0);
  
  Swr_refl = analogRead(SwrReflPin);
  refl = Swr_refl;
  if (refl > 999) refl = 999;
  lcd.print(" "); 
  if (refl < 100)  lcd.print("0");
  if (refl < 10)  lcd.print("0");
  lcd.print(refl, DEC); 
      
  if (capPos == 0){ 
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);             
                delay(60);
        digitalWrite(BuzzerPin, HIGH);  
                delay(30);
        digitalWrite(BuzzerPin, LOW);   
        goto endDown; 
        }
      
  servo.write(capPos);
    
  delay(100);  
  if (analogRead(manualTunePin) < 340) goto againDown; 
  
  endDown:
  digitalWrite(TuneTxPin, LOW);
 
  lcd.setCursor(0, 1); 
  lcd.print("  STBY  ");

  digitalWrite(servoPowerPin, LOW);
  servo.detach(); 
  delay(500);
 }