// RSSI testing
//            2 W .. 1 km..8 km.
//           25 mW .. 125 m .. 1 km
// MODE A.  From current position, move left, move right  by 5 degrees, get 3 RSSI, decide where to move
// MODE B.  Scan  
// At 500 meters:
// Off to side 80..82, zero visibility
// 95..102, a lot of interference
// 150..155  perfect visibility.
//modes:
// Manual: Button 3 and 4 are 0: The antenna is moved only by the sticks
// Mode A: 

const int Scan_Max_cnt=280;
byte  RSSI_Scan [Scan_Max_cnt]={};
int   Servo_Scan [Scan_Max_cnt]={};
byte  Max_Scan [Scan_Max_cnt]={};
byte  Min_Scan [Scan_Max_cnt]={};
int ndx;
int Best_RSSI_Servo;
int Worst_RSSI_Servo_A;
int Worst_RSSI_Servo_B;
int Autostep=0;

const long int start_recording_delay=45000; // 45000 = 45 seconds
long int recording_turn_off_time =0;
boolean  Recording_On=false;


const int Base_Elevation=2300;  // Antenna Position at startup without PPM stream
const int Base_Azimut=1500; // Antenna Position at startup without PPM stream 
const int Lost_Elevation=2000;  // Antenna Position if the PPM stream from RC is lost
const int Lost_Azimut=1500;  // Antenna Position if the PPM stream from RC is lost 

const byte PPMinPin = 8;  // Pin for PPM input, through resistor 600 Ohm

const byte Servo_CH1  = 9; 
const byte Servo_CH2  = 10; 
const byte RSSI_pin = A4;
const byte DVR_pin= 3;
//  RSSI MAX 241..242
//  RSSI MIN 80

volatile int Servos[4]={}; //  _1 =1500;   // 01000000
// volatile int Servo_2= 1100;    // 00100000
volatile int Servo_1_Target=1500;
volatile int Servo_2_Target=1100;
volatile int Servo_1_Speed=0;
volatile int Servo_2_Speed=0;
volatile int Servo_1_state=1;
volatile int Servo_2_state=1;
volatile int Servo_1_Offset=0;
long last_move_Servos=0;
int RSSI=0;

int Button_1=0;
int Button_2=0;
int Button_3=0;
int Button_4=0;

int cnt=0;

boolean Active_Scan=false;
const int Left_Roll_CH=0;
const int Left_Pitch_CH=1;
const int Button_CH=2;
const int Not_Used_CH=3;
const int Right_Roll_CH=4;
const int Right_Pitch_CH=5;
const int Trim_Pitch_CH=6;
const int Trim_Roll_CH=7;

const unsigned int Outframelength= 28000*2;  // Duration of the total Frame, in uS *2, usually 28000 * 2, must match Tx Setup
const int PPMoutnegativeduration =300*2;  // Duration of the Negative Pulse in uS * 2, usually 300 * 2, must match Tx Setup
const int LastValidChannel = 8;  // Number of channels which must be in the PPM stream to be considered valid (either 8 or 12), must match Tx Setup

const long int MaxPPMdelay=2000;   // Maximum delay in receiving a PPM sentence for the last PPM to be considered valid, in ms, 200 = 2 seconds

volatile int Channel = 0;  // Current Channel 

volatile boolean NeverPPM= true;
volatile boolean ValidPPM =false;
volatile boolean ErrorPPM =false;
volatile long int lastPPMupdate; // lasttime when the PPM was updated with a valid sentence (is checked by timer2 routine to reset ValidPPM
// Channelsin_buffer+pauses = buffer PPM IN (values in 2 * uS)   - used during reading of PPM
// Channels = Valid PPM in uS (including pause)  - complete PPM sentence. Accompanying variable: LastPPMUpdate (time)  
volatile  int Channelsin_buffer [] =   {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1000, 1000, 1000, 12, 120};
volatile  int Channelsin []=           {1500, 1500, 2213, 1500, 1500, 1500, 1100, 1500, 1500, 1500, 1500, 1200, 1000, 1000, 0, 0};

volatile unsigned int TCNTBase;
  
volatile int TM;
volatile uint16_t timertmp,timernow,timerlast,tm; 

volatile long int last_scan_time=0;

volatile int last_RSSI;
volatile int Servo_Start;
//-----------------------------------------------------
void setup() {

pinMode(PPMinPin, INPUT_PULLUP);  
pinMode(Servo_CH1, OUTPUT); 
pinMode(Servo_CH2, OUTPUT);   
digitalWrite (DVR_pin,LOW);
pinMode(DVR_pin, OUTPUT);   
digitalWrite (DVR_pin,LOW);


Servos[1]=Base_Azimut;
Servos[2]=Base_Elevation;
Servo_1_Target=Base_Azimut;
Servo_2_Target=Base_Elevation;
Button_3=0;

Button_4=0;

 ValidPPM =false;
 ErrorPPM = false;

 Serial.begin(115200);
 Serial.println ("DEBUGGING STARTED");
 delay (300);
 Serial.println ("");          
 Serial.println ("Ready");
  

 noInterrupts();
// timer1 SETUP (Used For Reading and Writing PPM Stream)     
  TCCR1C = B00000000;  
  TCCR1B = B10000010; //  TCCR1B |= (1 << ICNC1) | (0<<ICES1)  | (1<<CS11);  Noise Cancel, Falling Edge,  Prescaler = 8  
  TCCR1A = B00000000;   //  TCCR1A |= (1 <<COM1A1) | (1<<COM1A0) ;   O  
  OCR1A =  Servos[1]+Servos[1];
  OCR1B =  Servos[2]+Servos[2];
  TCNT1=0;  // Set timer to zero  
  TCNTBase=0; 
  TIMSK1= B00100110;  //  TIMSK1  |= (1 <<ICIE1) | (1<<OCIE1A) ; // Enable Interrupt on Capture, Enable Interrupt on Match B, Enable Interrupt on Match A
  interrupts();    
}
//-----------------------------------------------------

void read_buttons () {
 // if (not ValidPPM) {return;} 

int PWM=Channelsin [2];
if ((PWM>2213-35)  and (PWM<2213+35)) {Button_1=0;Button_2=0;Button_3=0;Button_4=0;}
if ((PWM>2133-35)  and (PWM<2133+35)) {Button_1=1;Button_2=0;Button_3=0;Button_4=0;}
if ((PWM>2053-35)  and (PWM<2053+35)) {Button_1=0;Button_2=1;Button_3=0;Button_4=0;}
if ((PWM>1973-35)  and (PWM<1973+35)) {Button_1=1;Button_2=1;Button_3=0;Button_4=0;}

if ((PWM>1893-35)  and (PWM<1893+35)) {Button_1=0;Button_2=0;Button_3=1;Button_4=0;}
if ((PWM>1814-35)  and (PWM<1814+35)) {Button_1=1;Button_2=0;Button_3=1;Button_4=0;}
if ((PWM>1734-35)  and (PWM<1734+35)) {Button_1=0;Button_2=1;Button_3=1;Button_4=0;}
if ((PWM>1654-35)  and (PWM<1654+35)) {Button_1=1;Button_2=1;Button_3=1;Button_4=0;}

if ((PWM>1574-35)  and (PWM<1574+35)) {Button_1=0;Button_2=0;Button_3=0;Button_4=1;}
if ((PWM>1494-35)  and (PWM<1494+35)) {Button_1=1;Button_2=0;Button_3=0;Button_4=1;}
if ((PWM>1415-35)  and (PWM<1415+35)) {Button_1=0;Button_2=1;Button_3=0;Button_4=1;}
if ((PWM>1335-35)  and (PWM<1335+35)) {Button_1=1;Button_2=1;Button_3=0;Button_4=1;}
}
//-----------------------------------------------------
int get_RSSI() {  // time required slightly less than 1 mS around 0.8 mS
  int acc=0;
  for(int i=0;i<8;i++) {acc=acc+analogRead(RSSI_pin);}
  return acc/8;
}
//-----------------------------------------------------
void move_Servos() {  
  if (Active_Scan) {return;}   // We block all movements during scan
  if (millis ()<last_move_Servos+10 ) {return;}  // We need this one to happen every 10 mS
  last_move_Servos=millis();

  int delta=Servo_1_Target-Servos[1];  
  int sign=0;
  if (delta<0)  {sign=-1;} else {sign=1;}
  delta=abs(delta);
  if (delta>=5) {Servo_1_Speed=Servo_1_Speed+4;}  // We increase the step by this factor
  if (Servo_1_Speed*15>delta) {Servo_1_Speed=delta/15+4; if (Servo_1_Speed<4) {Servo_1_Speed=4;}     }    
  Servos[1]=Servos[1]+sign*Servo_1_Speed;
  if (delta<10)  {Servos[1]=Servo_1_Target;Servo_1_Speed=0;}

  delta=Servo_2_Target-Servos[2];  // 1500 1700  -200
  sign=0;
  if (delta<0)  {sign=-1;} else {sign=+1;}
  delta=abs(delta);
  if (delta>=5) {Servo_2_Speed=Servo_2_Speed+4;}
  if (Servo_2_Speed*15>delta) {Servo_2_Speed=delta/15+4; if (Servo_2_Speed<4) {Servo_2_Speed=4;}     } //  500 
  Servos[2]=Servos[2]+sign*Servo_2_Speed;
  if (delta<10)  {Servos[2]=Servo_2_Target;Servo_2_Speed=0;}  
}
//-----------------------------------------------------
void Scan  (int Servo_ndx, int Servo_From, int Servo_To, int RSSI_limit, int steps_beyond_limit) {   // 
//  Examples:
// 

const  int Movespeed=12; // 10 millisecond for each step
const  int Movestep=8;  // 10 microseconds for each pulse, equal to about 1 degree, 7 uS is minimum (to prevent overrun), 12 uS is maximum 

Active_Scan=true;  
Serial.println ("=====   SCAN   ======");
if (Servo_ndx==1) {Serial.println ("Horizontal");} else {Serial.println ("Vertical");}
Servo_From=Servos[Servo_ndx]+Servo_From;
Servo_To=Servos[Servo_ndx]+Servo_To;

if (Servo_From<600)  {Servo_From=600;}
if (Servo_To>2400)  {Servo_To=2400;}

RSSI=get_RSSI();
if (RSSI<RSSI_limit+10)  {RSSI_limit=RSSI-10;  if (RSSI_limit<82) {RSSI_limit=82;}}  //  if we are in a situation with low RSSI, we reduce the RSSI limit downwards

int sbl=steps_beyond_limit;
Servo_Start=Servos[Servo_ndx];   // Preserve Current Servo Position

RSSI=1000;
while ((Servos[Servo_ndx]<Servo_To) and (RSSI>=RSSI_limit))  // Repeat until: you run to the left limit, or your RSSI goes below a predefined limit
{  Servos[Servo_ndx]=Servos[Servo_ndx]+Movestep;  // about 1 degree left
   delay (Movespeed);
   RSSI=get_RSSI();
}

while ((Servos[Servo_ndx]<Servo_To) and (sbl>0))  // after reaching the RSSI  limit, move another few predefined steps if the number of steps is defined.
{  Servos[Servo_ndx]=Servos[Servo_ndx]+Movestep;  // about 1 degree left
   delay (Movespeed);
   sbl=sbl-1;
 }
delay (Movespeed*3);   // Stop and allow time for backlash to stabilizde
ndx=0;
while (Servos[Servo_ndx]>=Servo_Start)  // move unconditionally untill the initial position (important if the RSSI changed slightly downwards)
  {Servos[Servo_ndx]=Servos[Servo_ndx]-Movestep; // move about 1 degree right
   delay (Movespeed);
   RSSI=get_RSSI(); if (RSSI>255) {RSSI=255;} // This is necessary should an overflow over one byte occur.
   RSSI_Scan [ndx]=RSSI;
   Servo_Scan [ndx]=Servos[Servo_ndx];
   ndx=ndx+1;   if (ndx>Scan_Max_cnt) {ndx=Scan_Max_cnt;}
  } 

while ((Servos[Servo_ndx]>Servo_From) and (RSSI>=RSSI_limit))  // move until RSSI goes below the preset level or the other predefined limit is reached
  {Servos[Servo_ndx]=Servos[Servo_ndx]-Movestep; // 
   delay  (Movespeed);
   RSSI=get_RSSI(); if (RSSI>255) {RSSI=255;}
   RSSI_Scan [ndx]=RSSI; 
   Servo_Scan [ndx]=Servos[Servo_ndx];
   ndx=ndx+1; if (ndx>Scan_Max_cnt) {ndx=Scan_Max_cnt;}
  }  
 
sbl=steps_beyond_limit;  
while ((Servos[Servo_ndx]>Servo_From) and (sbl>0))  // after reaching the RSSI  limit, move another few steps if the number of steps is defined.
{  Servos[Servo_ndx]=Servos[Servo_ndx]-Movestep;  // about 1 degree left
   delay (Movespeed);
   sbl=sbl-1;
   RSSI=get_RSSI(); if (RSSI>255) {RSSI=255;}
   RSSI_Scan [ndx]=RSSI; 
   Servo_Scan  [ndx]=Servos[Servo_ndx];
   ndx=ndx+1;    if (ndx>Scan_Max_cnt) {ndx=Scan_Max_cnt;}
 }
delay  (Movespeed*3);
while ((Servos[Servo_ndx]<Servo_Start))  // Bring back to the original position.
{Servos[Servo_ndx]=Servos[Servo_ndx]+Movestep;  // about 1 degree left
   delay(Movespeed);   
 }
Active_Scan=false;  
last_scan_time=millis(); //   globalseconds;
last_RSSI=0;  
}
//-----------------------------------------------------
void Scan_Analyze(float percent) {  // percent is the percentage of values to be used for the analysis as high

int max_RSSI=0;
int min_RSSI=1000;
for(int i=0;i<ndx;i++) { 
     if   (RSSI_Scan [i]>max_RSSI) {max_RSSI=RSSI_Scan [i];}
     if   (RSSI_Scan [i]<min_RSSI) {min_RSSI=RSSI_Scan [i];}
       }
for(int i=0;i<ndx;i++) { Max_Scan [i]=0;Min_Scan [i]=0;}

int rs=max_RSSI;
int cnt=ndx*percent;


Serial.print ("RSSI  max  min:");Serial.print (max_RSSI);Serial.print (" ");Serial.println (min_RSSI);
Serial.print ("Data Points:");Serial.print (ndx);Serial.print ("  Criteria Points:");Serial.println (cnt);

while (cnt>0) {
    for(int i=0;i<ndx;i++) {if (RSSI_Scan [i]==rs) {Max_Scan [i]=1;cnt=cnt-1;}}
    rs=rs-1;
    }


for(int i=0;i<ndx;i++) {if (Max_Scan [i]==1) {Serial.print ("x");} else {Serial.print ("-");} }
Serial.println("");

/*
rs=min_RSSI;
cnt=ndx*percent*2;  // cnt is the minimum number f datapoints
while (cnt>0) {
    for(int i=0;i<ndx;i++) {if (RSSI_Scan [i]==rs) {Min_Scan [i]=1;cnt=cnt-1;}}
    rs=rs+1;
    }

for(int i=0;i<ndx;i++) {if (Min_Scan [i]==1) {Serial.print ("0");} else {Serial.print ("-");} }
Serial.println("");
*/

if (ndx*percent>6)  { // if there are at least 6 values or more, we eliminate two from left, and two from right
  cnt=2; 
  for(int i=0;i<ndx;i++) {   // we eliminate the two leftmost values
           if (Max_Scan [i]==1) {Max_Scan [i]=0;cnt=cnt-1;}
           if (cnt==0) {break;}
           } 
  cnt=2;
  for(int i=ndx-1;i>0;i--) {  // we eliminate the two rightmost values
            if (Max_Scan [i]==1) {Max_Scan [i]=0;cnt=cnt-1;}  
            if (cnt==0) {break;}
       }       
}

for (int i=0;i<ndx;i++) {if (Max_Scan [i]==1) {Serial.print ("x");} else {Serial.print ("-");} }
Serial.println("");


int icnt=0;
cnt=0;
int onlyone=0;
for  (int i=0;i<ndx;i++) {if (Max_Scan [i]==1) {icnt=icnt+i;cnt=cnt+1;onlyone=icnt;}}
if ((cnt>0) and (cnt<=2))  {Best_RSSI_Servo=Servo_Scan [onlyone];}  // Special case, if there was only one or only two values, we use the second one.
if (cnt==0)  {Best_RSSI_Servo=Servo_Start;}  // Special case, if there were no results (should never happen), but we will set it to previous position, so as not to move.
if (cnt>2) {Best_RSSI_Servo=Servo_Scan [icnt/cnt];}
Serial.print ("Best Servo:");Serial.print (Best_RSSI_Servo);Serial.print ("with RSSI:");Serial.print (RSSI_Scan[icnt/cnt]);

}
//-----------------------------------------------------
//++++++++++++++++++++++++++++++++++++++++++++++++++++
void loop() {

if (not (Recording_On) and (millis()>start_recording_delay)) {digitalWrite(DVR_pin,HIGH);recording_turn_off_time=millis()+1000;Recording_On=true;}

if ((recording_turn_off_time>0) and (millis()>recording_turn_off_time)) {digitalWrite(DVR_pin,LOW);recording_turn_off_time=0;}


move_Servos();
read_buttons ();
if (millis()>(lastPPMupdate+MaxPPMdelay)) {ValidPPM=false;} // Check if valid PPM is still available. 
if ((not ValidPPM) and (NeverPPM==false)) {Servo_1_Target=Lost_Azimut;Servo_2_Target=Lost_Elevation;Button_1=0;Button_2=0;Button_3=0;Button_4=0;} // if RC PPM stream is disconnected from Tracker...

if ((Button_1==1) and (Button_3==1) and (Active_Scan==false))  { // Normal Correction button
     Scan  (1,-250,+250,90,6);
     Scan_Analyze(0.16);
     Servo_1_Target=Best_RSSI_Servo;
  }    

if ((Button_2==1) and (Button_3==1) and (Active_Scan==false))  { // More global button, bigger scan and in both axis
     Scan  (1,-500,+500,85,8);
     Scan_Analyze(0.16);
     Servo_1_Target=Best_RSSI_Servo;
     
     Scan  (2,-400,+400,85,0);
     Scan_Analyze(0.16);
     Servo_2_Target=Best_RSSI_Servo;
  }    

if (Button_4==1)  {  // Auto tracking Mode
  int RSSIn=get_RSSI();
  if ((millis()>last_scan_time+2000) and (last_RSSI==0)) {last_RSSI=RSSIn;} // We reset last_RSSI after 2 seconds.
     
  if (RSSIn<200)  // if RSSI is good, do not do anything at all, only if below this value
        {              
           if (millis()>last_scan_time+15000) {  //every 15 seconds we will try to do a soft correction
              Scan  (1,-140,+140,100,3);
              Scan_Analyze(0.18);
              Servo_1_Target=Best_RSSI_Servo;
              Serial.print ("  15 SECONDS SCAN, RSSI  was:"); Serial.println (RSSIn);
              Autostep=Autostep+1;
              if (Autostep>=3)               {
                    Scan  (2,-200,+200,95,3);
                    Scan_Analyze(0.16);
                    Servo_2_Target=Best_RSSI_Servo;
                    Autostep=0;
              }
           }
           
           if ((millis()>last_scan_time+6000) and (RSSIn<98)) {  //every 6 seconds if RSSI  very low
              Scan  (1,-380,+380,90,15);
              Scan_Analyze(0.18);
              Servo_1_Target=Best_RSSI_Servo; 
              Autostep=Autostep+1;
              if (Autostep>=4)               {
                    Scan  (2,-200,+200,95,3);
                    Scan_Analyze(0.16);
                    Servo_2_Target=Best_RSSI_Servo;
                    Autostep=0;
              }
              Serial.print ("  Low RSSI 6 seconds scan, RSSI  was:");Serial.println (RSSIn);                  
           }

          if (last_RSSI>RSSIn+25) {     // if there was a significant drop in RSSI, do scan
              Scan  (1,-370,+370,90,6);
              Scan_Analyze(0.16);
              Servo_1_Target=Best_RSSI_Servo; 
              Autostep=Autostep+1;
              Serial.println ("  DROP IN RSSI SCAN, RSSI  was:"); Serial.println (RSSIn);
              last_RSSI=0;                                          
          }
        } 
  }    

if ((Button_3==0) and (Button_4==0) and (ValidPPM))  {  // Direct Mode
     Servo_1_Target=1500-(int)(1.74*(float)(Channelsin [7]-1500));  // 2000 1000      1000 20009n   2000-1500  500  *1.4 = 700
     Servo_2_Target=Channelsin [6]+380;
     if (Servo_1_Target>2400)  {Servo_1_Target=2400;}
     if (Servo_1_Target<600)  {Servo_1_Target=600;} 
    }
}
// ----------------------------------------------
ISR(TIMER1_COMPA_vect)
{ 
  if (Servos[1]<600) {Servos[1]=600;}
  if (Servos[1]>2400) {Servos[1]=2400;}
  if (Servo_1_state==1)  
      {Servo_1_state=0; PORTB = PORTB & B11111011;OCR1A=OCR1A+20000-Servos[1]-Servos[1];} // Off, no pulse, 10 mS 
   else 
      {Servo_1_state=1;PORTB = PORTB |  B00000100; OCR1A=OCR1A+Servos[1]+Servos[1];}  //On, pulse
}
// ----------------------------------------------
ISR(TIMER1_COMPB_vect)
{
  if (Servo_2_state==1)  
      {Servo_2_state=0; PORTB = PORTB & B11111101;OCR1B=OCR1B+20000-Servos[2]-Servos[2];} // Off, no pulse
   else 
      {Servo_2_state=1;PORTB = PORTB |  B00000010; OCR1B=OCR1B+Servos[2]+Servos[2];}  //On, pulse
}
// ---------------------------------------------------------------------
ISR(TIMER1_CAPT_vect)
{
 timerlast=timernow;   
 timernow=ICR1;  // Falling Edge Only !! No Positive Edge Checks!!!

 if (timernow<timerlast) {tm = timernow + (65536-timerlast);} else {tm=timernow-timerlast;} // Overflow Condtion  

 if  ((tm<=1500) or ((tm>4800) and (tm<=6000)))  // 750 uS or less or 2400 - 3000 uS, it means the signal is ERROR
         {Channelsin_buffer [Channel] = tm; // save length of the pulse
          Channel = Channel + 1;  
          ErrorPPM=true;      
          if  (Channel>LastValidChannel) {Channel=LastValidChannel;}                
         } 

if  ((tm>1500) and (tm<=4800))   // 750 uS or less or 2400, it means the signal is a servo pulse
         {Channelsin_buffer [Channel] = tm; // save length of the pulse, 1 ms = 2000 counts (8 prescaler)  2 ms = 4000 cycles
          Channel = Channel + 1;        
          if  (Channel>LastValidChannel) { Channel=LastValidChannel;ErrorPPM=true;}     
         } 
          
if  (tm>6000)  // 6000 = 3 ms  (0,002*16MHz/8 prescale)   if time is longer than 3 ms, it means that the signal is a framing signal
    {   
           if (Channel==LastValidChannel) // Check if Number of channels received is correct
                {if (!ErrorPPM) 
                        {ValidPPM=true;  // If there were no errors, then the whole sentence is valid, 
                         NeverPPM=false;
                        lastPPMupdate=millis(); //set time when this valid PPM was received
                        for(int i=0;i<LastValidChannel;i++) {Channelsin [i]=Channelsin_buffer[i]/2;}  // put PPM data into Definitive Input buffer                                               
                        } 
                  ErrorPPM=false; // Reset error flag for next sentence
                 }
              else 
                {ValidPPM=false;ErrorPPM=false;}  // Not a valid PPM, reset Error Flag for next sentence
            Channel = 0; // set the next channel to be read as channel 0         
        } 
}