#include "PWM.h"

/*

 Mimics the fade example but with an extra parameter for frequency. It should dim but with a flicker
 because the frequency has been set low enough for the human eye to detect. This flicker is easiest to see when
 the LED is moving with respect to the eye and when it is between about 20% - 60% brighness. The library
 allows for a frequency range from 1Hz - 2MHz on 16 bit timers and 31Hz - 2 MHz on 8 bit timers. When
 SetPinFrequency()/SetPinFrequencySafe() is called, a bool is returned which can be tested to verify the
 frequency was actually changed.

 This example runs on mega and uno.
 */

int32_t frequency = 20000; //Frequency of the PWM to the H-Bridge in Hz
#define pwmpin1 9
#define pwmpin2 10
#define buckboostpin 3
#define opto_pin 4
#define ammeterpin A0
#define voltmeterpin A1

double analogValue;
double analogValue1, analogValue2;
double wavePeriod = 20000; //Period of 1 sine wave in microseconds, 50Hz -> 20ms/wave -> 20 000 micro seconds
double hardOffset = 8600; //Number of microseconds the optocoupler input lags behind grid sine                    //EXTEND TO INCLUDE FILTER AND OTHER DELAY

int opto_prevVal = 0, opto_curVal = 0;
double opto_totalTime;
double opto_t0, opto_t1, opto_startTime;
int opto_counter = 0, opto_counterLimit = 5; //The higher this value, the more accurate the average wavePeriod becomes
int opto_measureFlag = 0;
double opto_syncDelay = 0, opto_syncDelayDiff = 0;
double avgPhaseTimeOffset = 0;
double currentPhaseTimeOffset = 0;

double prevPower = 0, curPower;
double buckboostDutyCycle = 10.0;

void setup() {
  Serial.begin(115200);
  InitTimersSafe();
  //sets the frequency for the specified pin
  bool success1 = SetPinFrequencySafe(pwmpin1, frequency);
  bool success2 = SetPinFrequencySafe(pwmpin2, frequency);
  bool success3 = SetPinFrequencySafe(buckboostpin, frequency);

  if((success1) && (success2) && (success3))
    Serial.println("Frequency of all pins configured correctly");
  else
  {
    if(!(success1))
      Serial.println("Frequency of pin 1 not set correctly");
    if(!(success2))
      Serial.println("Frequency of pin 2 not set correctly");
    if(!(success3))
      Serial.println("Frequency of pin 3 not set correctly");
  }
}

void loop() {

  //--------------------------------------vv Phase locked loop vv--------------------------------------
  opto_curVal= digitalRead(opto_pin);
  if(opto_curVal == 1 && opto_prevVal == 0) //Check when optocoupler signal goes high
  {
    if(opto_measureFlag == 0) //Starting the time counter
    {
      opto_t0 = micros();
      avgPhaseTimeOffset = avgPhaseTimeOffset + (1/(double(opto_counterLimit)))*sin(2*3.1415*(opto_t0 - opto_syncDelay)/wavePeriod);
      opto_measureFlag = 2;
    }
    if(opto_measureFlag == 1) //Check when optocoupler signal goes high (again) after timer is started and after signal has gone low
    {
      opto_t1 = micros();
      opto_measureFlag = 0;
      opto_totalTime = opto_totalTime + (opto_t1 - opto_t0);
      opto_counter++;
      if(opto_counter == opto_counterLimit)
      {
        //Sync the delay between startpoints (opto_SyncDelay) of grid wave and H-Bridge wave and the difference in frequency of the two waves (->Correcting H-Bridge PWM frequency wavePeriod)
        opto_syncDelay = opto_syncDelay + avgPhaseTimeOffset*1000;//syncDelay is in microseconds, this line shifts the delay left or right with a value that decreases as the gap closes
        //wavePeriod = wavePeriod - (wavePeriod - opto_totalTime/5)*0.01; //If waveperiod of grid is larger than wavePeriod, wavePeriod increases by difference * factor

        opto_counter = 0;
        opto_totalTime = 0;
        avgPhaseTimeOffset = 0;
      }
    }
  }
  if(opto_measureFlag == 2 && opto_prevVal == 1 && opto_curVal == 0) //Check when optoCoupler signal goes LOW
    opto_measureFlag = 1;

  opto_prevVal = opto_curVal;



  //--------------------------------------^^ Phase locked loop ^^--------------------------------------
  //--------------------------------------vv H-Bridge control vv--------------------------------------

  analogValue = 255*sin(((double(micros())+(wavePeriod - opto_syncDelay - hardOffset))/wavePeriod)*2*3.141592);
  //H-Bridge consists of 2 drivers controlling either the top or bottom half of sinewave. Idle driver receives 0, working driver recieves (positive) SPWM
  if(analogValue > 0)
  {
    analogValue2 = 0;
    analogValue1 = analogValue;
  }
  else
  {
    analogValue1 = 0;
    analogValue2 = -analogValue;
  }
  pwmWrite(pwmpin1, analogValue1);
  pwmWrite(pwmpin2, analogValue2);

  //--------------------------------------^^ H-Bridge control ^^--------------------------------------
  //--------------------------------------vv Buck/Boost converter control and MPPT vv--------------------------------------
  /*
  if(opto_counter == 3) //Value is arbitrarily chosen to spread the calculations
  {
    //curpower = current * voltage
    curPower = ((2.52 - (double(analogRead(ammeterpin))*(5/1023)) * 1.112) * (double((analogRead(A1)));               //DEFINE MEASURING RANGES OF BOTH VOLTMETER AND AMMETER
    if(prevPower > curPower)
      buckboostDutyCycle = buckboostDutyCycle + 0.1;                                                                  //SIGNS PROBABLY INCORRECT -> DEPENDS ON BUCK OR BOOST CONVERTER
    if(prevPower < curPower)
      buckboostDutyCycle = buckboostDutyCycle - 0.1;
    pwmWrite(buckboostpin, buckboostDutyCycle);
    prevPower = curPower;

    //EXTEND CODE TO INCLUDE OPTIMIZATION FOR HARDPHASEDELAY (Alternating? Simultaneously? Consecutively?)
  }
  */
  //--------------------------------------^^ Buck/Boost converter control and MPPT ^^--------------------------------------


}