/*
    This program generates 3 PWM sine waves, each offset by 120 degrees.
    The lookup array (sine66[]) is 110 values long. The 1st phase runs from
    0 to 65. The 2nd phase starts at offset 22 (1/3 of 66) and the 3rd phase 
    starts at offset 44 (44 + 66 = 110). The number 66 was an arbitrary decision, 
    suitable for our application. The sine wave looks fine, and it's easy on the
    hardware.

*/
#include <Arduino.h>
#include "Globals.h"
// #include "Timers.h"

/*
PROGMEM const uint8_t sine66[] =
{
127,139,151,163,174,185,196,205,215,223,230,237,242,247,250,253,254,254,253,250,247,243,    // start phase 1
237,231,223,215,206,196,185,174,163,151,139,127,115,103,91,80,69,59,49,40,31,24,            // start phase 2
17,12,7,4,1,0,0,1,4,7,11,17,23,31,39,48,58,68,79,91,103,115,                                // start phase 3
127,139,151,162,174,185,195,205,214,223,230,237,242,247,250,253,254,254,253,251,247,243,
237,231,223,215,206,196,186,175,163,152,140,128,116,104,92,80,69,59,49,40,31,24
};

#define BLINK_OUTP      33
#define PWM_OUT_1       12
#define PWM_OUT_2       13
#define PWM_OUT_3       14
#define ENABLE_IN       25
#define SCOPE_OUT       26  // scope trigger
#define SPEED_IN        32  // potentiometer, ADC1_0

#define OFFSET_2        22  // offset for second-phase
#define OFFSET_3        44  // offset for third-phase
#define PWM_MAX         66
#define RESET           0

int iScanCounter = 0;
int iSpeed;
char cBuff[30];
char cOutput[200];
bool bPrint = false;
bool bToggle = false;
unsigned long lInterval = 1000L;
unsigned long lLoopTimer;
unsigned long lLoopDelay = 2000;
*/

/////////////////////////////////////////////////////////////////
void task_one_second();
void task_pwm_phasing();
// #define _TASK_SCHEDULING_OPTIONS
// #define _TASK_TIMECRITICAL
// #define _TASK_MICRO_RES
// #define _TASK_SLEEP_ON_IDLE_RUN
#include <TaskScheduler.h>
Scheduler userScheduler;    // to control tasks
Task taskResetCount(TASK_SECOND, TASK_FOREVER, &task_one_second);
// Task taskResetCount(TASK_SECOND, TASK_FOREVER, &task_one_second, &userScheduler, true);
// Task taskPWMPhasing(lInterval, TASK_FOREVER, &task_pwm_phasing);
// Task taskPWMPhasing(lInterval, TASK_FOREVER, &task_pwm_phasing, &userScheduler, true);

/////////////////////////////////////////////////////////////////
void task_one_second() {
    // char *  itoa ( int value, char * str, int base );
    itoa(iScanCounter, cBuff, 10);
    strcpy(cOutput, "Scan Count = ");
    strcat(cOutput, cBuff);

    // if (iScanCounter < 500) 
    //     lLoopDelay -= 5;
    // if (iScanCounter > 500)
    //     lLoopDelay += 5;
    iScanCounter = 0;

    // itoa(lLoopDelay, cBuff, 10);
    // strcat(cOutput, "   Loop Delay = ");
    // strcat(cOutput, cBuff);

    itoa(iSpeed, cBuff, 10);
    strcat(cOutput, "    Speed = ");
    strcat(cOutput, cBuff);

    itoa(lInterval, cBuff, 10);
    strcat(cOutput, "    Interval = ");
    strcat(cOutput, cBuff);
    bPrint = true;

    bToggle = !bToggle;
    if (bToggle == true)
        digitalWrite(BLINK_OUTP, HIGH);
    else
        digitalWrite(BLINK_OUTP, LOW);
}

/////////////////////////////////////////////////////////////////
void task_pwm_phasing() {
    static int iCnt1 = -1;
    static int iCnt2 = OFFSET_2 - 1;    // 22 - 1
    static int iCnt3 = OFFSET_3 - 1;    // 44 - 1

    if (digitalRead(ENABLE_IN) == HIGH) {       // if switched ON
      digitalWrite(SCOPE_OUT, HIGH);
      // digitalWrite(ONBOARD_LED, HIGH);
      iCnt1++;
      if (iCnt1 >= PWM_MAX) {         // max = 66
          iCnt1 = 0;                  // reset everything
          iCnt2 = OFFSET_2;
          iCnt3 = OFFSET_3;
      }
      else {
          iCnt2++;                    // increment
          iCnt3++;
      }
      // get pwm value from array and send to output pins
      analogWrite(PWM_OUT_1, sine66[iCnt1]);  
      analogWrite(PWM_OUT_2, sine66[iCnt2]);
      analogWrite(PWM_OUT_3, sine66[iCnt3]);
    }
    else {                              // if not ON
        analogWrite(PWM_OUT_1, 0);
        analogWrite(PWM_OUT_2, 0);
        analogWrite(PWM_OUT_3, 0);
    }

    // get new interrupt interval. controls motor speed
    // taskPWMPhasing.setInterval(lInterval);
    // taskPWMPhasing.restartDelayed();

    digitalWrite(SCOPE_OUT, LOW);
    // digitalWrite(ONBOARD_LED, LOW);
}

/////////////////////////////////////////////////////////////////
bool timer_phasing_micros(unsigned long lTime) {
    static unsigned long lTimeLast = 0L;
    static unsigned long lStartTime = 0L;
    if (lTimeLast != lTime) {
        lTimeLast = lTime;
        lStartTime = micros();
    }
    // subtraction method avoids problems at rollover
    if (micros() - lStartTime >= lTime) {
        return true;
    }
    return false;
}

/////////////////////////////////////////////////////////////////
void setup() {

    Serial.begin(115200);
    while (!Serial)
        delay(10);    // wait for serial port to open

    pinMode(BLINK_OUTP, OUTPUT);
    // digitalWrite(BLINK_OUTP, HIGH);

    pinMode(SCOPE_OUT, OUTPUT);
    pinMode(PWM_OUT_1, OUTPUT);     // PWM output / frequency output
    pinMode(PWM_OUT_2, OUTPUT);     // PWM output / frequency output
    pinMode(PWM_OUT_3, OUTPUT);     // PWM output / frequency output
    pinMode(ENABLE_IN, INPUT);

Serial.println("Scheduler starts");
    // taskResetCount.setSchedulingOption(TASK_SCHEDULE);
    userScheduler.init();
    // userScheduler.addTask(taskPWMPhasing);
    // taskPWMPhasing.enable();
    userScheduler.addTask(taskResetCount);
    taskResetCount.enable();

    // userScheduler.startNow();   // This creates a new scheduling starting point for all ACTIVE tasks. 
    //                             // PLEASE NOTE - THIS METHOD DOES NOT ACTIVATE TASKS, JUST RESETS THE START TIME
    // taskPWMPhasing.delay();     // Tasks which need to start delayed, need to be delayed again after startNow();
    // taskResetCount.delay();

    lLoopTimer = micros();
}

//////////////////////////////////////////////////////////////////////
void loop() {
    
    userScheduler.execute();

    iSpeed = analogRead(SPEED_IN);      // 0 to 4095
    lInterval = map(iSpeed, 0, 4095, 10000L, 100L);    // reverse speed to micros
    
    // until task_pwm_phasing() works on scheduler, use micro timer
    if (timer_phasing_micros(lInterval)) {
        timer_phasing_micros(RESET);      // requires reset

        task_pwm_phasing();
    }
        
    if (bPrint) {
        bPrint = false;
        Serial.println(cOutput);
    }

    iScanCounter++;
    while ((micros() - lLoopTimer) < lLoopDelay)      // 2000 should equal 500 scans/sec
        ;
    lLoopTimer = micros(); // reset loop timer
}