/////ATSlight/////
/*
TOOLS
  DEBUG
  CLI
  SIGNAL Stimulus similator for testing

//TBD COMPONENTS
  *EEPROM parameters loadfromEE, set and savetoEE
  I/O local
  *IO Expander
  Debounce
  LED pattern (buzzer)
  PWM on any pin
  PLC Framework ladder logic
  i2c LCD 

  *LCD Menue
  *Serial Menu
  rs485

  *wifi
  *BT
  RFtranciever 
  *RTC, SD

*/

#include <TimerOne.h>
//CLI Stuff goes here
///// DEBUG STUFF ////////////////////////////////
//////////////////////////////////////////////////
class BPDEBUGGER{
// Define the callback function signature
typedef void (*CallbackFunction)(String s);

#define BPDEBUGGERDOC "https://docs.google.com/document/d/1n0r7_nqzqog4-LxEut5lkxUAUwDTn0wvq2oc11CFJAY/edit"
#define DEBUG   true
//#define DEBUG   false
#if DEBUG
#define NL  {Serial.print("\n");}
#define PRINTS(s)   { Serial.print(F(s)); }
#define PRINT(s,v)  { Serial.print(F(s)); Serial.print(v); NL }
#define PRINTX(s,v) { Serial.print(F(s)); Serial.print(v, HEX); NL}
#define BP        bp.breakpoint(__LINE__);
#define BPx(func)  func; BP;
#define BPS(s)     NL PRINTS(s) BP;
#define BPSV(s,v)  NL PRINT(s,v) BP;
#define BPDELAY(n) bp.bpdelay = n;BP
#else
#define NL  
#define PRINTS(s)  
#define PRINT(s,v)  
#define PRINTX(s,v)
#define BP        
#define BPx(func)  
#define BPS(s)    
#define BPSV(s,v)  
#define BPDELAY(n)
#endif
//DEBUG varaiables
public:
CallbackFunction callback;
unsigned long bpstarttime, bpdelay = 1000;//bp timers millis
unsigned long bpcontinuetime, bptimebetweenbps;//bp timers micros 
bool buttonpressed=false, bpxcmd = false;//bp exit command
bool isatbreakpoint;//true when bp is in effect. can control isr activity
String bpmessage = " This is a Breakpoint Help Message \r\n documentation at BPDEBUGGERDOC";
  BPDEBUGGER(){

  }
  void breakpoint(int bpline){
    PRINT("BREAKPOINT Line ", bpline);
    isatbreakpoint = true;
    bpstarttime = millis();//global
    //unsigned long bpcontinuetime,bptimebetweenbps; //global
    bptimebetweenbps = micros()-bpcontinuetime;//
    printwatchlist();// global function
    while(true){
      //readinputcommand();
      if (bpcontinue()) {
        //save breakpoint exit time
        bpcontinuetime=micros();
        isatbreakpoint = false;
        return;
      }
    }   
  }
  void help(){Serial.println(BPDEBUGGERDOC);}
  void printwatchlist(){}
  void readinputcommand(){}
  bool bpcontinue(){
    //check if timeout
    if (millis()>bpstarttime + bpdelay){
      if (bpdelay==0) return false;
      Serial.println("\nBreakpoint Timeout .. Continuing\n");
      return true;
    }
    //check if bpx command
    if (bpxcmd) {
      bpxcmd = false;//consume the bpxcmd flag
      Serial.println("\nBreakoint Continue command received .. Continuing\n");
      return true;
    }
    //check if continue button is pressed
    if(buttonpressed){
      delay(20);buttonpressed=false;  
      Serial.println("\nBreakpoint Continue Button Pressed .. Continuing\n");
      return true;
    }
    return false;
  }
};
///////////// END OF DEBUG STUFF //////////////

/////////////////////////////////////////////////////////////////
class EnergyMonitorATS{
  #define ADC_COUNTS  (1<<ADC_BITS)
  #define ADC_BITS    10
  public:
  unsigned int crossCount, numberOfSamples, maxnumberOfSamples ;
//    double realPower,
//      apparentPower,
//      powerFactor,
//      Vrms,
//      Irms,
//      frequency,
//      energy,
//      offset,
//      lastaverageoffset=512;

  double Vrms, frequency, offset, lastaverageoffset=512;
 
  int samples[60];//FOR DEBUGGING<
  int samplesindex;//FOR DEBUGGING<<
  //int isrcount;//FOR DEBUGGING<<
  //private:
  unsigned long t1u, t2u, tdeltau;//FOR DEBUGGING TIMING <  
  unsigned long lastfcheck;//last frequency check time in ms <
  //unsigned long lastEnergyCalcTime;//last energy calculation time<

  unsigned int offsetpin;// pin to read the offset <
  unsigned int inPinV;//voltage pin
  //unsigned int inPinI;//current pin
  //Calibration coefficients
  double VCAL;
  //double ICAL;
  double PHASECAL;  
 
  //int sampleV, sampleI;//value read by adc
  int sampleV;//value read by adc
  //double filteredV,filteredI;
  double filteredV;  
  //double lastFilteredV,lastFilteredI,phaseShiftedV;
  double lastFilteredV,phaseShiftedV;
  //double sqV,sumV,sqI,sumI,instP,sumP;
  double sqV,sumV;
  bool lastVCross, checkVCross;

  void isr(){
    digitalWrite(13, HIGH);//FOR DEBUGGING<
    t1u = micros();//FOR DEBUGGING<
 
    numberOfSamples++;
    lastFilteredV = filteredV;               //Used for delay/phase compensation
    //SAMPLE
    lastaverageoffset = offset;//TO SMOOTH OUT OFFSET<
    offset = analogRead(offsetpin);
    offset = (offset + lastaverageoffset)/2;
    sampleV = analogRead(inPinV);
    //sampleI = analogRead(inPinI);//NOT NEEDED FOR VOLTAGE ONLY CLASS
   
    //FILTER
    filteredV = sampleV - offset;//<
    //filteredI = sampleI - offset;//<//NOT NEEDED FOR VOLTAGE ONLY CLASS
    samples[samplesindex]=filteredV;//FOR DEBUGGING<<
    samplesindex++;//FOR DEBUGGING<<    
   
    //-----------------------------------------------------------------------------
    // C) Root-mean-square method voltage
    //-----------------------------------------------------------------------------
    sqV= filteredV * filteredV;                 //1) square voltage values
    sumV += sqV;                                //2) sum  
    //-----------------------------------------------------------------------------
    // D) Root-mean-square method current //NOT NEEDED FOR VOLTAGE ONLY CLASS
    //-----------------------------------------------------------------------------
    //sqI= filteredI * filteredI;                 //1) square voltage values
    //sumI += sqI;

    //PHASE CALIBRATION (E)-------------------------
    phaseShiftedV = lastFilteredV + PHASECAL * (filteredV - lastFilteredV);

    //INSTANTANEOUS POWER CALC  (F)------------------
    //instP = phaseShiftedV * filteredI;          //Instantaneous Power
    //sumP +=instP;  

    // Zero Crossing Detection
    //uses lastVcross, checkVcross, crossCount
    //----------------------------------
    lastVCross = checkVCross;
    if (sampleV > offset) checkVCross = true;
                     else checkVCross = false;
    if (numberOfSamples==1) lastVCross = checkVCross;
    if (!lastVCross && checkVCross){
      //zero crossing
      crossCount++;
      calcVI();//<      
    }

    //isrcount++;//FOR DEBUGGING<<
    t2u = micros();//FOR DEBUGGING<<
    digitalWrite(13, LOW);//FOR DEBUGGING<<
  }//END OF ISR
 
  void calcVI(){
    //Calculation of the root of the mean of the voltage and current squared (rms)
    //Calibration coefficients applied.
    //long SupplyVoltage = 3300;// 3.3 V SUPPLY
    long SupplyVoltage = 5000;//5 V SUPPLY
   
    //RMS Voltage calc
    double V_RATIO = VCAL *((SupplyVoltage/1000.0) / (ADC_COUNTS));
    Vrms = V_RATIO * sqrt(sumV / numberOfSamples);
    //RMS Current calc
    //double I_RATIO = ICAL *((SupplyVoltage/1000.0) / (ADC_COUNTS));//NOT NEEDED FOR VOLTAGE ONLY CLASS
    //Irms = I_RATIO * sqrt(sumI / numberOfSamples);//NOT NEEDED FOR VOLTAGE ONLY CLASS
 
    //Calculation power values
    //realPower = V_RATIO * I_RATIO * sumP / numberOfSamples;//NOT NEEDED FOR VOLTAGE ONLY CLASS
    //apparentPower = Vrms * Irms;//NOT NEEDED FOR VOLTAGE ONLY CLASS
//    if (apparentPower!=0){
//      powerFactor=realPower / apparentPower;//NOT NEEDED FOR VOLTAGE ONLY CLASS
//    }else{
//      powerFactor = 1;
//    }
    //Energy Calculation in Ws <<
//    unsigned long energyTime = millis() - lastEnergyCalcTime;
//    if (energyTime>50) energyTime=0;
//    lastEnergyCalcTime = millis();
//    //IF THIS POWER SOURCE IS SUPPLYING LOAD
//    // if(loadIsOnMe){}
//    energy += realPower * energyTime;
   
    /////////////////////////////////////////////////////
    //Reset accumulators
    sumV = 0;
    //sumI = 0;
    //sumP = 0;
    maxnumberOfSamples = numberOfSamples;////FOR DEBUGGING<<
    numberOfSamples=0;//<
    //COUNT CROSSINGS IN ONE SECOND<
    if (millis()-lastfcheck>1000){
      lastfcheck = millis();
      frequency = 1.0 * crossCount;
      crossCount=0;      
    }      
    samplesindex=0;////FOR DEBUGGING<<    
  }//////////  End of calcVI  ////////////////////////////////
  //--------------------------------------------------------------------------------------
  // Sets the pins to be used for voltage and current sensors
  //--------------------------------------------------------------------------------------
  void voltage(unsigned int _inPinV, double _VCAL, double _PHASECAL)
  {
    inPinV = _inPinV;
    VCAL = _VCAL;
    PHASECAL = _PHASECAL;
    offset = ADC_COUNTS>>1;
  }  
  void current(unsigned int _inPinI, double _ICAL)
  {
//    inPinI = _inPinI;
//    ICAL = _ICAL;
//    offset = ADC_COUNTS>>1;
  }
  void offsetpinsetup(unsigned int _offsetpin){
    offsetpin = _offsetpin;//<
  }
  void serialprint(){
    //FOR DEBUGGING<< prints out values of an array
    /*Serial.print(maxnumberOfSamples );Serial.println(" Samples ");//FOR DEBUGGING<<
    Serial.println(" ---Filtered Voltage samples--- ");
    for(int n=0; n<maxnumberOfSamples;n++){
      Serial.print(n);Serial.print(", ");Serial.println(samples[n]);
    }
    Serial.println(" --- end --- ");*/
    /////////////////////////////////////////////////////////
    Serial.print(Vrms);
    Serial.println(" V ");
    Serial.print(frequency);
    Serial.println(" Hz ");
//    Serial.print(Irms);
//    Serial.println(" A ");                
//    Serial.print(realPower);
//    Serial.println(" W ");
//    Serial.print(apparentPower);
//    Serial.println(" VA ");
//    Serial.print(powerFactor);
//    Serial.println(" pf ");    
//    Serial.print(energy);
//    Serial.println(" Ws ");
    Serial.println("--------------------------\r\n");
  }
};//////////    END OF EnergyMonitorATS CLASS   //////////////////////////////

//////////////////////////////////////////////////////////////////////////////
// VARIABLES
unsigned long t1u, t2u, tdeltau;//FOR DEBUGGING<<
EnergyMonitorATS emonats1;
EnergyMonitorATS emonats2;
unsigned long isrt1, isrexectime, isrperiod, lastisrtime;//FOR DEBUGGING<<

BPDEBUGGER bp;
//////////////////TIMER2 OVERFLOW ISR ////////////////
ISR(TIMER2_OVF_vect) {
/*  isrperiod = micros()-lastisrtime;//FOR DEBUGGING<<
  lastisrtime = micros();//FOR DEBUGGING<<
  isrt1=micros();//FOR DEBUGGING<<
 
  emonats1.isr();
  emonats2.isr();
 
  TCNT2 = 200;
 
  isrexectime = micros()-isrt1;//FOR DEBUGGING<<
*/  
}
///////////////////////////////////////////////////////////
/////////////////// SETUP /////////////////////////////////
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(13, OUTPUT);

  ADCsetup();
  //Timer2setup();
  TimerOneSetup();
   
  emonats1.voltage(A0, 134.00, 0.0);  // Voltage: input pin, calibration, phase_shift
  //emonats1.current(A1, 111.1);       // Current: input pin, calibration.
  emonats1.offsetpinsetup(A2);

  emonats2.voltage(A3, 134.00, 0.0);  // Voltage: input pin, calibration, phase_shift
  //emonats2.current(A1, 111.1);       // Current: input pin, calibration.
  emonats2.offsetpinsetup(A2); 
  //bp.help();delay(5000);
  bp.callback= myCallback;

}
////////////////////// LOOP //////////////////////////////
void loop() {
  // put your main code here, to run repeatedly:
 
  //emonats1.tdeltau = emonats1.t2u - emonats1.t1u;
  //Serial.print("tovisr exec time = ");Serial.println(isrexectime);
  //Serial.print("isr period = ");Serial.println(isrperiod);
  Serial.println("Source1 Printout -------------");
  emonats1.serialprint();
  Serial.println("Source2 Printout -------------");  
  emonats2.serialprint();
  Serial.println("");
  bp.callback("Hi Ed ");
  BP
}
void TimerOneSetup(){
  Timer1.initialize(500);
  Timer1.attachInterrupt(ISRcode);
}
void Timer2setup(){
  // Set Timer 2 to normal mode
  TCCR2A = 0;
  TCCR2B = 0;
  // Set Timer 2 to 8-bit mode and enable overflow interrupt
  TCCR2A |= (1 << WGM21); // Set Timer/Counter Control Register A for normal operation
  TIMSK2 |= (1 << TOIE2); // Enable Timer/Counter2 Overflow Interrupt
  // Set the Timer 2 Prescaler to 8 for 500 microseconds interrupt
  TCCR2B |= (1 << CS21) | (1 << CS20) ; // Set Timer/Counter Control Register B for prescaler of 8
  // Enable interrupts
  sei();  
}
void ADCsetup(){
  ADCSRA |= (1<<ADPS2); //set
  ADCSRA &= ~(1<<ADPS1); //clear
  ADCSRA &= ~(1<<ADPS0); //clear  
}
void printwatchlist(){
  Serial.println("--------------- WATCH LIST -----------------");
  //PRINT("Time between last BPs (us) ", bptimebetweenbps )
  //PRINT("LOOPEXECTIME between last BPs (us) ", LOOPEXECTIME )  
  //PRINT("isr exec time = (us) ", isrexectime)  
  PRINT("isr period = (us) ", isrperiod)
  PRINT("V1 ",emonats1.Vrms )
  PRINT("f1 ",emonats1.frequency )
  PRINT("samples ",emonats1.maxnumberOfSamples )
  PRINT("V2 ",emonats2.Vrms )
  PRINT("f2 ",emonats2.frequency )
  Serial.println("------------ END OF WATCH LIST -------------");     
}
void ISRcode(){
  isrperiod = micros()-lastisrtime;//FOR DEBUGGING<<
  lastisrtime = micros();//FOR DEBUGGING<<
  isrt1=micros();//FOR DEBUGGING<<

  if(!bp.isatbreakpoint){
    emonats1.isr();
    emonats2.isr();
  }

  isrexectime = micros()-isrt1;//FOR DEBUGGING<<  
}
// Callback function to be called by MyObject
void myCallback(String str) {
  Serial.print(str);Serial.println("Callback function called!");
}