/*
  Antenna Rotor Control
  ArduinoHZRotor Project
  Copyright Dr. Pedro E. Colla (2013) LU7HZ
  Free for radioamateur usages
*/

//*=====================================[Constants]============================================
#include <SoftwareSerial.h>
#define MAX       8
#define MAXSIZE   8
#define BLINK    50
#define MARGIN   10
#define ONESEC   1000

//*--------------------------------------------------------------------------------------------
//* Pin Assignments
//*
//* A0 = analog rotor position (0..5V).
//* A1 = analog reference (semi-auto operation)
//* D2 = rotor turn command (CW)
//* D3 = rotor turn command (CCW)
//* D4 = manual rotor turn command (CW)
//* D5 = manual rotor turn command (CCW)
//* D6 = operation mode (0=semi-auto disabled / 1=semi-auto enabled)
//*--------------------------------------------------------------------------------------------

const int led          = 13;
const int rotorPin     = A0;
const int autoPin      = A1;
const int CWPin        = 2;
const int CCWPin       = 3;
const int buttonCW     = 4;
const int buttonCCW    = 5;
const int buttonSET    = 6;
const int buttonAUTO   = 7;

String  cmdline = String(8);
char    str1[8] = {'\0'};
char    NUL[1]  = {'\0'};
char    CR      = 0x0d;
char    LF      = 0x0a;
char    EOM     =';';

//*--- Establish electrical parameters for rotor
float   VMIN    =  0.0;
float   VMAX    =  5.0;
float   VMID    = VMAX/2;
float   VROTOR  = 0.0;
float   VREF    = 0.0;
float   VAUTO   = 0.0;
float   VSTEP   = 0.033;
float   INC     = 0.0;
float   autoValue=0.0;

//*--- Parameters for Az=f(V) regression model (Az=K*V+B)

float   K       =  72.0;
float   B       = 180.0;

//*--- Low pass filter for rotor voltage response

float a[8];

boolean stat;
int i           = 0;
//int az          = 0; 
int c           = 0;
boolean ROTOR   = false;
boolean flash   = false;
boolean MANUAL  = false;
boolean AUTO    = false;

//*--- movement targets

int tmin;
int t;
int tmax;

int rotorValue  = 0;
int refValue    = 0;

long lastBlink = 0;   
boolean statBlink = false;
boolean autoflag=false;
int lastAP;
int azauto;
int azRef;

//*--- Debounce logic variables
int  buttonSETState     = HIGH;
long lastSETDebounceTime= 0;  // the last time the output pin was toggled
int  lastSETState       = HIGH;

int  buttonAUTOState     = HIGH;
long lastAUTODebounceTime= 0;  // the last time the output pin was toggled
int  lastAUTOState       = HIGH;


int  buttonCWState       = HIGH;
long lastCWDebounceTime  = 0;
int  lastCWState         = HIGH;

int  buttonCCWState      = HIGH;
long lastCCWDebounceTime = 0;
int  lastCCWState        = HIGH;

long debounceDelay       = 50;    // the debounce time; increase if the output flickers


//*=====================================[Setup]================================================
void setup () {

// Define I/O specs  
  pinMode(led, OUTPUT);
  pinMode(CWPin,OUTPUT);
  pinMode(CCWPin,OUTPUT);
  pinMode(buttonCW,INPUT);
  pinMode(buttonCCW,INPUT);
  pinMode(buttonSET,INPUT);
  
// Open serial communications and wait for port to open:
  Serial.begin(4800);
  
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Leonardo only
  }

// Define general purpose variables
  stat=false;
  cmdline=NUL;
  t=0;
  resetROTOR();
  
// Initialize low pass filter to smooth out rotor response  
  
  i=0;
  while (i<MAX) {
    a[i]=VROTOR;
    i++;
  }
  
}  
//*=====================================[Main Loop]============================================
void loop () {

//*--------------------------------------------------------------------------------------------
//* Process DCU-1 Commands thru the serial port
//*--------------------------------------------------------------------------------------------
   if (Serial.available()) {
      char ct=Serial.read();
      if (ct == CR) {
         ct = EOM;
      }
      
//*--------------------------------------------------------------------------------------------
//* Parse command, interpret and execute
//*--------------------------------------------------------------------------------------------
      if (cmdline.length() == 0) {
         
         if (ct == EOM || ct == 'a' || ct == 'A' || ct == 's' || ct == 'S') {
            cmdline += ct;
            if (ct == EOM) {
               ProcessCommand();
               cmdline = NUL;
            }   
         }
         
      } else {
         
        if (cmdline.length() >= MAXSIZE) {
           cmdline = NUL;
         } else {           
            cmdline +=ct;
            cmdline.toUpperCase();
            if (ct == EOM) {
               ProcessCommand();
               cmdline = NUL;
            }   
         }  
      }  
      
  }    
//*--------------------------------------------------------------------------------------------
//* Read current rotor status and update
//*--------------------------------------------------------------------------------------------
  updateAZ();          //*DEBUG fake update as the rotor isn't there during debug
  updateRotor();
//*--------------------------------------------------------------------------------------------
//* Operate actuators depending on the status (automatic)
//*--------------------------------------------------------------------------------------------
  if (ROTOR == true) {
    if (INC == +1.0) {
       digitalWrite(CWPin,HIGH);
       digitalWrite(CCWPin,LOW);
     } else {
       digitalWrite(CWPin,LOW);
       digitalWrite(CCWPin,HIGH);
    } 
    digitalWrite(led,HIGH);
  } else {
    digitalWrite(led,LOW);
    digitalWrite(CWPin,LOW);
    digitalWrite(CCWPin,LOW);
    resetROTOR();
  }
//*--------------------------------------------------------------------------------------------
//* Operate actuators depending on the status (manual)
//*--------------------------------------------------------------------------------------------
  if (digitalRead(buttonCW) == LOW) {
      digitalWrite(CWPin,HIGH);
      digitalWrite(CCWPin,LOW);
      digitalWrite(led,HIGH);
      INC=+1.0;
      ROTOR=true;
      MANUAL=true;
  }

  if (digitalRead(buttonCCW) == LOW) {
      digitalWrite(CWPin,LOW);
      digitalWrite(CCWPin,HIGH);
      digitalWrite(led,HIGH);
      INC=-1.0;
      ROTOR=true;
      MANUAL=true;
  }
//*--------------------------------------------------------------------------------------------
//* Reset actuators when manual buttons are released
//*--------------------------------------------------------------------------------------------
  if (digitalRead(buttonCCW) == HIGH && digitalRead(buttonCW) == HIGH && MANUAL == true ) {
      digitalWrite(CWPin,LOW);
      digitalWrite(CCWPin,LOW);
      digitalWrite(led,LOW);
      resetROTOR();
      MANUAL=true;
  }  
  
//*--------------------------------------------------------------------------------------------
//* Sense the AUTO mode
//*--------------------------------------------------------------------------------------------
/*
  // read the state of the switch into a local variable:

  // check to see if you just pressed the button 
  // (i.e. the input went from LOW to HIGH),  and you've waited 
  // long enough since the last press to ignore any noise:  

  // If the switch changed, due to noise or pressing:
  if (reading != lastAUTOState) {
    // reset the debouncing timer
    lastAUTODebounceTime = millis();
  } 
  
  if ((millis() - lastAUTODebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:
*/
    int reading = digitalRead(buttonSET);

    // if the button state has changed:
    if (reading != buttonSETState) {
       buttonSETState = reading;
       if (buttonSETState == LOW) {
          digitalWrite(led,HIGH);
       } else {
          digitalWrite(led,LOW);
       }   
    }
  
  
//*--------------------------------------------------------------------------------------------
//* Read secondary reference (used in AUTO mode to set the bearing)
//*--------------------------------------------------------------------------------------------
  updateReference();

//*--------------------------------------------------------------------------------------------
//* Verify the AUTO mode trigger
//*--------------------------------------------------------------------------------------------
    reading = digitalRead(buttonAUTO);

    // if the button state has changed:
    if (reading != buttonAUTOState) {
       buttonAUTOState = reading;
       if (buttonAUTOState == LOW) {
          digitalWrite(led,HIGH);
          AUTO=true;
       } else {
          if(AUTO==true) {
             AUTO=false;
             azRef=V2Az(VREF);
             setTarget(azRef);
             updateRotor();
             //digitalWrite(led,LOW);
          }  
       }   
    }
  
  
}
//*=========================================================================================
// Process a command stream
// commands supported
// AI1;      Host query controller about the current azimuth
// AP1xxx;   Move rotor to azimuth xxx
// AM1;      Starts towards the last AP command
// ST1;      Stop immediately
//*=========================================================================================
void ProcessCommand() {
  
  char buf[8];
  cmdline.toCharArray(buf,8);
  
//*-- Process ";" command, this HALT the rotor  
  
  if (strncmp(buf,";",1) == 0) {
    digitalWrite(led,LOW);
    resetROTOR();
    return;
  }

//*-- Process "AI1;" command, query about current rotor Azimuth

  if (strncmp(buf,"AI1;",4) == 0) {
      if (buttonSETState == LOW) {
         azRef=V2Az(VREF);
         sendAZ(azRef);
      } else {
         sendAZ(getAZ());
      }   
        return;
  }

//*--- Process "ST1;" command, Halt the rotor immediately

  if (strncmp(buf,"ST1;",4) == 0) {
     digitalWrite(led,LOW);
     resetROTOR();
     return; 
  }

//*--- Process "AM1;" command, start movement towards the last azimuth set by AP1 command

  if (strncmp(buf,"AM1;",4) == 0) {
    setTarget(lastAP);
    ROTOR=true;
    return;
  }
  
//*--- Process "AP1nnn;" command, where nnn is the azimuth in degrees [0..359]

  if (strncmp(buf,"AP1",3) == 0) { 
     int p = 5;
     int x = 1;
     int W1= 0;
     while (p>=3) {
       if (buf[p] >= '0' && buf[p] <= '9') {
          W1 = W1 + (x * (buf[p]-'0'));
       } else {
         break;   
       }
       x=x*10;
       p--;
     }
     if (W1 >= 0 && W1 <= 360) {  
        setTarget(W1);
        lastAP=W1;
     }   
     updateRotor();
     return;
  }
}
//*=========================================================================================
// Format and send the Azimuth
//*=========================================================================================
void sendAZ(int azimuth) {
       String szAZ = String(azimuth);
       szAZ += ";";
       Serial.println(szAZ);
}
//*==========================================================================================
//* setTarget
//* Establish the target (t) and computes the minimum and max bound for it (tmin/tmax)
//*=========================================================================================
void setTarget (int target) {
 
 if (target <= MARGIN) {
    t    = MARGIN;
    tmin = 0;
    tmax = 2*MARGIN;
 } else {
    if (target >= (360-MARGIN)) {
        t    = 360-MARGIN;
        tmin = 360-(2*MARGIN);
        tmax = 360;
    } else {
        tmin = target - MARGIN;
        tmax = target + MARGIN;
        t    = target;
    }
 }

INC=setRotation();
}  
//*==========================================================================================
//* setRotation
//* set the rotation direction +1 is CW and -1 is CCW
//*=========================================================================================
float setRotation() {
 
    float rotation = 0.0; 
    int az=getAZ();
    
    if (t<=180) {
       if (az<=180) {
          if (t>az) {
             rotation=+1.0;
          } else {
             rotation=-1.0;
          }
       } else {
         rotation=+1.0;
       }  
        
    } else {

       if (az>180) {
          if (t>az) {
             rotation=+1.0;
          } else {
             rotation=-1.0;
          }
       } else {
         rotation=-1.0;
       }  
      
    }  
    
    return rotation;
    
}

//*==========================================================================================
//* updateRotor
//* verify if the rotor already reach the azimuth required
//*=========================================================================================
boolean updateRotor() {
  
  int  azimuth=getAZ();
  
  if (MANUAL==false) {
     if (azimuth >= tmin && azimuth <= tmax) {
        digitalWrite(led,LOW);
        resetROTOR();
     } else {
       ROTOR=true;
     }
  }   

  return ROTOR;  
 
}
//*=========================================================================================
// Get the current Az
//*=========================================================================================
int getAZ() {
  
  
//*--- Compute the current rotor Azimuth and convert it to degrees

    int azimuth=V2Az(getAverage());
    return azimuth; 
}
//*=========================================================================================
// Update the reference azimuth
//*=========================================================================================
void updateReference() {

  refValue = analogRead(autoPin);
  VREF = refValue * (5.0 / 1023.0);
          
  if (VREF<=VMIN) {
     VREF=VMIN;
  }

  if (VREF>=VMAX) {
     VREF=VMAX;
  }

}  
//*=========================================================================================
// Update the azimuth
//*=========================================================================================
void updateAZ() {

  rotorValue = analogRead(rotorPin);
  VROTOR = rotorValue * (5.0 / 1023.0);
   
//*--- DEBUG
  if (ROTOR == true) {  //* ERASE AFTER TEST
     //VROTOR = VROTOR+(INC*VSTEP);  // Esto debe ser reemplazado por la lectura del rotor
  }   

// Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 5V):
     //
          
  if (VROTOR<=VMIN) {
     VROTOR=VMIN;
     resetROTOR();
  }

  if (VROTOR>=VMAX) {
     VROTOR=VMAX;
     resetROTOR();
  }

//* Update low pass filter with new value

  i=0;
  while(i<(MAX-1)) {
    a[i]=a[i+1];
    i++;
  }
  a[7]=VROTOR;  
  
}
//*=========================================================================================
// Reset ROTOR
//*=========================================================================================
void resetROTOR() {
 
  ROTOR=false;
  tmin=0;
  tmax=360;
  INC=0;
  
}  
//*=========================================================================================
// Convert voltage read from rotor into azimuth
//*=========================================================================================
int V2Az(float V) {
  
  float azimuth = 0.0;
  
  if (V >= VMIN && V <= VMID) {
     azimuth= K*V+B;
     
     if (azimuth==180) {
        azimuth=181;
     }
  } else {
     azimuth= K*V-B;
     if (azimuth==180) {
        azimuth=179;
     }   
  }

  return int(azimuth);  
}  
//*=========================================================================================
// Return weighted average of Vref (equivalent to a 8 pole low pass filter)
//*=========================================================================================
float getAverage() {
  
   i=0;
   float sum=0.0;
   while (i<MAX) {
      sum=sum+a[i];
      i++;
   }   
   sum=sum/MAX;
   return sum;
}