/*
 * ARDUINO STATE MACHINE 2022 01
 * 
 */
//#include <SoftwareSerial.h>   
#include "SerialCommand.h"

#include <avr/sleep.h>.
#include <avr/wdt.h>

#define VERSION "ARDUINO STATE MACHINE 2022 01"


#define DEBUG 0
#if(DEBUG)
#define DEBUGP(x) Serial.print(x)        
#define DEBUGN(x) Serial.println(x)
#else
#define DEBUGP(x) 
#define DEBUGN(x) 
#endif

SerialCommand SCmd;   // The demo SerialCommand object


//HZ Control 
#define HZ_SETTING   20
int mainLoop_count;
unsigned long fast_loopTimer;                          // Time in miliseconds of main control loop
const int hzCount = (1000 / HZ_SETTING)-1;
const int timeRemind = 1000 / HZ_SETTING;

void(* resetFunc) (void) = 0; //declare reset function @ address 0 call resetFunc(); if you want to reset
#define  LED_PIN   13 
#define  LED_DEMO 3 



int LED_HZ = 2;
int HZ_COUNT = HZ_SETTING;

 void hw_init(){
    Serial.begin(115200); 
    pinMode(LED_PIN,OUTPUT);
    pinMode(LED_DEMO,OUTPUT);
 }
 


void process_command()    
{
  int aNumber,bNumber;  
  char *arg; 

  Serial.println("We're in process_command"); 
  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    aNumber=atoi(arg);    // Converts a char string to an integer
    Serial.print("First argument was: "); 
    Serial.println(aNumber); 
    
  
  } 
  else {
    Serial.println("No arguments"); 
    aNumber =999;
  }

  arg = SCmd.next(); 
  if (arg != NULL) 
  {
    bNumber=atol(arg); 
    Serial.print("Second argument was: "); 
    Serial.println(bNumber); 
    
  } 
  else {
    Serial.println("No second argument"); 
    bNumber =999;
  }



  switch(aNumber){
    case 0 :
      Serial.println("0");
    break;
    case 1 :
      Serial.println("1 HZ");
      LED_HZ =1 *2;      
    break;
    case 2 :
      Serial.println("2 HZ");
      LED_HZ =2 *2;
    break;
    case 5 :
      Serial.println("5 Hz");
      LED_HZ =5 *2; 
    break;
    case 10 :
      Serial.println("10 Hz");
      LED_HZ =10 *2; 
    break;
    case 104 :
      Serial.println(VERSION);
    break;
    default:
      Serial.println("default 這個參數沒有實作喔!");
    break;
  }
}


// This gets set as the default handler, and gets called when no other command matches. 
void unrecognized()
{
  Serial.println("What? 沒有註冊這個指令"); 
}



void setup() {


     
//Setup I/O

     hw_init();

  

// register CMD     
     SCmd.addCommand("P",process_command);   
     SCmd.setDefaultHandler(unrecognized);   
      
     
//setup WDT
  
            wdt_enable(WDTO_4S);
            Serial.println(VERSION);
            Serial.println("請key 入下列指令, 然後按下Enter");
            Serial.println("P,1 ");
            Serial.println("P,2");
            Serial.println("P,5");
            Serial.println("P,10");
            Serial.println("使用者也可以鍵入入其他指令看看系統的反應");
            
            

}

 
 
int  secCount =0;
///Test Pattern is ON for 60 SEC, OFF for 180 Sec 


void loop() {
  // put your main code here, to run repeatedly:
     SCmd.readSerial();    

   // system Loop  HZ define in  #define HZ_SETTING   20

   if (millis()-fast_loopTimer >  hzCount) {
                     fast_loopTimer                = millis();
                     mainLoop_count++;
                     wdt_reset(); // Reset WDT ... 
                      
      /// do things
               
    if(mainLoop_count%HZ_SETTING==0){

      secCount++; 
      digitalWrite(LED_BUILTIN,!digitalRead(LED_BUILTIN));
      DEBUGP("Sec Count :");     
      DEBUGN(secCount);    

               
               
   }             
  
  
  
  ///執行改變變數之後的對應功能
  if(mainLoop_count% (HZ_COUNT/LED_HZ) ==0){

     
      digitalWrite(LED_DEMO,!digitalRead(LED_DEMO));
  }
    // Time Remind for this Loop -----------------------------------------                 // DEBUGP("Time Remind  ms :");
    // DEBUGN(timeRemind - (millis()-fast_loopTimer));
     }

}