#include <Arduino.h>
// NeoPixel Ring simple sketch (c) 2013 Shae Erisson
// released under the GPLv3 license to match the rest of the AdaFruit NeoPixel library
#include <avr/sleep.h>
#include <avr/power.h>
#include <avr/wdt.h>    /* Header for watchdog timers in AVR */
#include <Button2.h>
//#include <ezButton.h>
//#include <BobaBlox.h>
#include <Adafruit_NeoPixel.h>
//#include "pitches.h"
#include <SerialCommands.h>

#define DBG            1    //no sleep if debug 1
#define STRIPONOFF     0    //strip led power on/off
#define VLCD5_LET_ON   1    //if ON LET IT ON
#define BLINK_INH_VAL 15    // this value decrement by TIMER1 10Hz

#define PIN_IN_RIGHT   2    //470 Ohm  K.yellow
#define PIN_IN_LEFT    3    //470 Ohm  K.black
#define PIN_DOUT       4    //470 Ohm  GREEN Which pin on the Arduino is connected to the NeoPixels?
#define PIN_BUZZ_OUT   5    //470 Ohm //PWM 3*,5,6,9,10,11* ; *function will interfere with PWM output on pins 3 and 11
#define PWR_KRMILO     6    //470 Ohm  en3v3  (before red )
#define PIN_PWR_STRIP  7    //470 Ohm  Power strip
#define PIN_IN_BRAKE   8    //470 Ohm  don't use  !!!! i don+t know why?

#define PIN_RLED_PGM   9    //470 Ohm  R.white

#define PIN_VLCD5_ISON_A1 A1  //A0//2N7002  green - always check before the action is taken
#define PIN_VLCD5_DN_A0   A0  //12//2N7002  braun - only available if VLCD5 power ON
#define PIN_VLCD5_UP_12   12  //11//2N7002  white - only available if VLCD5 power ON
#define PIN_VLCD5_PWR_11  11  //10//BSS123  red   if VLCD5 is ON long pulse VLCD5_POFF to OFF; if VLCD5 is OFF then pulse VLCD5_PULSE to ON; if VLCD5 is ON then pulse VLCD5_PULSE to LIGTH ON  
#define PIN_BUTT_IN_10    10  //button on head unit
#define PIN_IN_BRAKE1      A2 

Button2 button(PIN_BUTT_IN_10);  // create Button2 object that attach to pin 10;

#define BUZZ_FREQ          300  //Hz
#define BUZZ_DUR            10  //ms
#define LONGPRESS         1200  //ms
#define ASSIST_TOUT       4*60*2  //sec 
#define VLCD5_T_PULSE     20   //ms
#define VLCD5_T_LPULSE    200  //ms
#define VLCD5_T_PIDLE     20   //ms
#define VLCD5_T_POFF      3000 //ms
#define VLCD5_T_PWRIDLE   5000 //ms
#define CNT_INH_WDT_RST   0    //0 mean that avery watchdog ISR event go to reset  (8s3)
#define VLCD5_T_KA        2520 // TIMER1 interupt 2520 x 0,1 = cca 4,2 min - how many time to sleep before KA to VLCD5
#define VLCD5_T_PWR_NEXT  100  // 100 x WDG(32ms) = cca  6 sec - how many time to sleep before KA to VLCD5
#define VLCD5_T_CMD_NEXT  10

#define PIN_LED_INTERNAL  13    //internal LED - it is used by PIN_VLCD5_ISON_A1 so it indicate the bike Battery is ON

#define VLCD5_M_UNKNOW     0 
#define VLCD5_M_DISPLAY    1 
#define VLCD5_M_LIGTH      2 
#define VLCD5_M_ASSIST     3 
#define VLCD5_M_DONE       4 
#define VLCD5_M_KA         5 

#define RED_MODE_OFF     0     // krmilnik off
#define RED_MODE_1       1     // 
#define RED_MODE_2       2     // 
#define RED_MODE_3       3     // 
#define RED_MODE_4       4     // 
#define RED_MODE_5       5     // 
#define RED_MODE_6       6     // 
#define RED_MODE_7       7     // krmilnik scan

#define NUMPIXFRONT   51
#define NUMPIXFBACK   137   // How many NeoPixels are attached to the Arduino?
#define NUMPIXELS     51 + 137  // How many NeoPixels are attached to the Arduino?
#define BRIGHTNESS  250
#define MODE_OFF      0
#define MODE_RIGHT    1
#define MODE_LEFT     2
#define MODE_NORM     3

//LED INDEX   D[ 0: 1: 2: 3: 4: 5  6: 7: 8: 9:10:11 12:13:14:15:16:17 18:19:20:21:22:23:] 
//LED POSITION: LP_BFR             LP_BRF             LP_BRB             LP_BBR

#define LP_BFR     0
#define LP_BRF     15        //6
#define LP_BRB     15+37     //12
#define LP_BBR     15+37-1  //18   // ends with 23

#define LP_BM      68       //ceil(137/2)
#define LP_BRM     15+16+1
#define LP_BBM     15+37+16
#define LP_BLM     15+37+33+18
#define LP_BFRM    7
#define LP_BFLM    15+37+33+37+7
#define LP_FRM     137+13-2 
#define LP_FLM     137+13+25+2

//LED INDEX   L[24:25:26:27:28:29 30:31:32:33:34:35 36:37:38:39:40:41 42:43:44:45:46:47]
//LED POSITION: LP_BBL             LP_BLB             LP_BLM             LP_BFL
#define LP_BBL     15+37+33-1    //24
#define LP_BLB     15+37+33  //30

#define LP_BFL     15+37+33+37 //42   // ends with 138//47
#define LP_BLST    137
//LED INDEX  DD(:48:49:50) 
//LED INDEX   D(:51:52:53:54:55:56:57:58:59:60:61:62:63:64:65:66:67:68:69:70) 
//LED INDEX   S(:71:72:73:74:75) 
//LED INDEX   L(:76:77:79:80:81:82:83:84:85:86:87:88:89:90:90:91:92:93:94:95)
//LED INDEX  LL(:96:97:98)
//LED POSITION:
#define LP_FFST   137   //48 
#define LP_FRR    137+3   //48  //3x  ends with 50
#define LP_FRB    137+3+20   //51  //20x  desni smerokaz košara end with 70
#define LP_FRF    137+3+20+2   //71  //2x
#define LP_FM     137+25   //73  //1x
#define LP_FLF    137+3+20+2+1+2   //74  //2x
#define LP_FLB    137+3+20+2+1+2+20   //76  //20x  levi  smerokaz košara end with 95
#define LP_FLL    137+3+20+2+1+2+20+3   //96  //3x   ends with 98
#define LP_FLST   137+3+20+2+1+2+20+3   //98  //

volatile uint8_t  stripPwr;
unsigned long     currentMillis;
unsigned long     button_Millis;
unsigned long     melodyMillis;

uint8_t btn10_ison = LOW;


//https://learn.adafruit.com/multi-tasking-the-arduino-part-2/timers
class Flasher {
	// Class Member Variables
	// These are initialized at startup
  
	int ledPin; // the number of the LED pin
	unsigned long OnTime;     // milliseconds of on-time
	unsigned long OffTime;    // milliseconds of off-time
 
  // Constructor - creates a Flasher 
  // and initializes the member variables and state
  public:
 	int ledPwr;              // ledPwr used to power the LED strip
  int change;
  	// These maintain the previus state
  unsigned long prevMillis;
  int play_dur = 1000;
  char* note = (char*)"A1";

  Flasher(int pin, long on, long off) {
    ledPin = pin;
    pinMode(ledPin, OUTPUT);     
      
    OnTime = on;
    OffTime = off;

    ledPwr = LOW; 
    prevMillis = 0;
  }

  void Update(unsigned long currMillis) {
    
    // check to see if it's time to change the state of the LED
    if((ledPwr == HIGH) && ((currMillis - prevMillis) >= OnTime)) {
      //Serial.print("!");
    	ledPwr = LOW;  // Turn it off
      if(STRIPONOFF) digitalWrite(ledPin, ledPwr);    // Update the actual LED
      prevMillis = currMillis;  // Remember the time
      change = 1;
    }else if ((ledPwr == LOW) && ((currMillis - prevMillis) >= OffTime)) {
      //Serial.print("x");
      ledPwr = HIGH;  // turn it on
      if(STRIPONOFF) digitalWrite(ledPin, ledPwr);    // Update the actual LED
      prevMillis = currMillis;   // Remember the time
      change = 1;
    }
  }
};

void setKrmilnik(uint8_t locmode) {
  digitalWrite(PWR_KRMILO, LOW);  delay(10); digitalWrite(PWR_KRMILO, HIGH);
  switch(locmode){
    case 0:
      break;
    default:
      for (uint8_t i = 0; i < locmode; i++) {
        digitalWrite(PIN_RLED_PGM, LOW ); delay(8); 
        digitalWrite(PIN_RLED_PGM, HIGH); delay(8);
      }
      break;
  }
}

volatile int     f_wdt = 1;
volatile uint8_t iFL1, iFL2, iFL3, iFL4;
volatile uint8_t iAL,  iAL_cnt = 0;
volatile uint8_t prevbuttstate, currbuttstate, preMode, curMode;
volatile uint16_t modeCnt;
volatile uint8_t vlcd5Mode = VLCD5_M_UNKNOW;
volatile uint8_t light;
volatile uint8_t iAssistLevelNow  = 2; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO 
volatile uint8_t iAssistLevelWant = 2; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO 
volatile uint8_t bAssistLevelDir  = 1; // 1- direction up
volatile uint8_t bAssistLevelViaL = false; //
volatile uint8_t bAssistLevelViaH = false; //
volatile uint8_t bAssistDNgo = false; //
volatile uint8_t bAssistUPgo = false; //

volatile uint16_t iAssistCnt = 0;
volatile uint16_t ivlcd5ka;
//volatile uint16_t inhvlcd5;

Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMPIXELS, PIN_DOUT, NEO_GRB + NEO_KHZ800);

volatile uint32_t yellow = strip.Color(255, 255, 0);
volatile uint32_t orange = strip.Color(230,80,0);
volatile uint32_t red =    strip.Color(255, 0, 0);
volatile uint32_t green =  strip.Color(0, 255, 0);
volatile uint32_t blue =   strip.Color(0, 0, 255);
volatile uint32_t white =  strip.Color(255, 255, 255);
volatile uint32_t pink =   strip.Color(255,0,100);
volatile uint32_t cyan =   strip.Color(0,255,255);
volatile uint32_t dark =   strip.Color(0, 0, 0);
volatile uint8_t  LedDirON =0, LedPosON =0;          //
volatile uint8_t  bright = 255;
volatile uint32_t whiteL =  strip.Color(4, 4, 4);
volatile uint32_t redL =    strip.Color(8, 0, 0);
volatile uint32_t blueL =   strip.Color(0, 0,10);
volatile uint16_t iwdt = CNT_INH_WDT_RST;
volatile uint8_t  giMid, giOfs, giWL, giDL, giFL, gidiv;
int imelody = 0;
unsigned int meduration = 0;
//// notes Mary Had a Little Lamb:
//int melody[] = {_E4,_D4};
//int medurs[] = { 4,  4};
//int imemax = 2;

//// notes Mary Had a Little Lamb:
//int melody[] = {_E4,_D4,_C4,_D4, _E4,_E4,_E4, _D4,_D4,_D4, _E4,_G4,_G4, _E4,_D4,_C4,_D4, _E4,_E4,_E4,_E4, _D4,_D4,_E4,_D4,_C4};
//int medurs[] = { 4,  4,  4,  4,   4,  4,  2,   4,  4,  2,   4,  4,  2,   4,  4,  4 , 4,   4,  4,  4,  4,   4,  4,  4,  4,  1};
//int imemax = 26;

//// notes Old MacDonald Had A Farm:
//int melody[] = {_G4,_G4,_G4,_D4, _E4,_E4,_D4, _B4,_B4,_A4,_A4, _G4,_D4,_D4, _G4,_G4,_G4,_D4, _E4,_E4,_D4, _B4,_B4,_A4,_A4, _G4};
//int medurs[] = { 4,  4,  4,  4,   4,  4,  2,   4,  4,  4,  4,   2,  4,  4,   4,  4,  4,  4,   4,  4,  2,   4,  4,  4,  4,   2};
//int imemax = 25;


// notes Old MacDonald Had A Farm:
////int melody[] = {_G5,_G5,_G5,_D5, _E5,_E5,_D5, _B5,_B5,_A5,_A5, _G5,_D5,_D5, _G5,_G5,_G5,_D5, _E5,_E5,_D5,_B5,_B5,_A5,_A5, _G5};
////int medurs[] = { 4,  4,  4,  4,   4,  4,  2,   4,  4,  4,  4,   2,  4,  4,   4,  4,  4,  4,   4,  4,  2,  4,  4,  4,  4,   2};
////int imemax = 25;

//// notes London Bridge is Falling Down:
//int melody[] = {_G4,_A4,_G4,_F4, _E4,_F4,_G4, _D4,_E4,_F4, _E4,_F4,_G4, _G4,_A4,_G4,_F4, _E4,_F4,_G4, _D4,_G4, _E4,_C4};
//int medurs[] = { 4,  4,  4,  4,   4,  4,  2,   4,  4,  2,   4,  4,  2,   4,  4,  4,  4,   4,  4,  2,   2,  2,   2,  2 };
//int imemax = 24;

// Declarations
Flasher StripNormal(PIN_PWR_STRIP, 200, 800);
Flasher StripLR    (PIN_PWR_STRIP, 400, 400);
//Flasher StripNormal(PIN_PWR_STRIP, 1000, 1000);
//Flasher StripLR    (PIN_PWR_STRIP, 1000, 1000);
//Speaker piezo(PIN_BUZZ_OUT);       // Declare a speaker using digital pin #2

//const unsigned int btnPin = PIN_BUTT_IN_10;
int timer1_counter;
#define SETT1_0s1   5
uint8_t timer1_1s = SETT1_0s1;

char cli_buffer_[32];
SerialCommands cli_(&Serial, cli_buffer_, sizeof(cli_buffer_), "\r\n", " ");

//This is the default handler, and gets called when no other command matches. 

void cmd_unrecognized(SerialCommands* sender, const char* cmd) {
  sender->GetSerial()->print("Unrecognized command [");
  sender->GetSerial()->print(cmd);
  sender->GetSerial()->println("]");
}
void cmd_note(SerialCommands* sender) {
  sender->GetSerial()->println("Note is played");
}
void cmd_set_dur(SerialCommands* sender) {
  sender->GetSerial()->println("Play duration is set");
}

SerialCommand cmd_note_("NOTE", cmd_note);
SerialCommand cmd_set_dur_("DUR", cmd_set_dur);
/***********************  SETUP runs once ******************************/
void setup() {
  // noTone(PIN_BUZZ_OUT);
  // put your setup code here, to run once:
  Serial.begin(57600);
  Serial.print("Initialising..."); delay(20);

  pinMode(PIN_VLCD5_ISON_A1, INPUT_PULLUP);
  digitalWrite(PIN_VLCD5_DN_A0, LOW);  pinMode(PIN_VLCD5_DN_A0,   OUTPUT);   // to disable power up spike
  digitalWrite(PIN_VLCD5_UP_12, LOW);  pinMode(PIN_VLCD5_UP_12,   OUTPUT); 
  digitalWrite(PIN_VLCD5_PWR_11, LOW); pinMode(PIN_VLCD5_PWR_11,  OUTPUT); 

  pinMode(PIN_IN_LEFT,  INPUT_PULLUP);
  pinMode(PIN_IN_RIGHT, INPUT_PULLUP);
  pinMode(PIN_IN_BRAKE, INPUT_PULLUP); //????!!!
  
  //pinMode(PIN_BUTT_IN_10, INPUT_PULLUP);  //allready defined as class Button!!! 
  pinMode(PWR_KRMILO,   OUTPUT); digitalWrite(PWR_KRMILO,   HIGH);
  pinMode(PIN_PWR_STRIP,OUTPUT); digitalWrite(PIN_PWR_STRIP, LOW);
  pinMode(PIN_RLED_PGM, OUTPUT); digitalWrite(PIN_RLED_PGM, HIGH);
  pinMode(PIN_LED_INTERNAL, OUTPUT); digitalWrite(PIN_LED_INTERNAL, LOW);
 
  digitalWrite(PIN_PWR_STRIP, HIGH);    // Update the actual LED

  /*** Setup the WDT ***/
  MCUSR &= ~(1 << WDRF);  /* Clear the reset status flag. */
  /* In order to change WDE or the prescaler, we need to
   * set WDCE (This will allow updates for 4 clock cycles).
 */
 /* The Arduino UNO has three timers
    Timer0 - An 8 bit timer used by Arduino functions delay(), millis() and micros().
    Timer1 - A 16 bit timer used by the Servo() library
    Timer2 - An 8 bit timer used by the Tone() library */
  // initialize timer1
  //https://www.hobbytronics.co.uk/arduino-timer-interrupts
  noInterrupts();       // disable all interrupts
  //Count to two preset values specified in Output Compare Register A or B (OCR1A, OCR1B).
  TCCR1A = 0;// set entire TCCR0A register to 0
  TCCR1B = 0;// same for TCCR0B

  //Set timer1_counter to the correct value for our interrupt interval
  //timer1_counter = 64911;   // preload timer 65536-16MHz/256/100Hz
  //timer1_counter = 64286;   // preload timer 65536-16MHz/256/50Hz
  //timer1_counter = 62411;   // preload timer 65536-16MHz/256/20Hz
    timer1_counter = 59286;   // preload timer 65536-16MHz/256/10Hz
  //timer1_counter = 34286;   // preload timer 65536-16MHz/256/2Hz
  TCNT1 = timer1_counter;   // preload timer
  //Timer1 can use a prescalar or increment based on input from an i/o pin (rising/falling edge).
  //  Prescalar divides the cpu clock by: off, 1, 8, 64, 256, 1024.
  TCCR1B |= (1 << CS12);    // 256 prescaler
  TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
  Serial.println(" complete."); delay(20); //Allow for serial print to complete. 
  strip.begin();            // This initializes the NeoPixel library.
  strip.show();             // initialize all pixels to 'OFF'
  
  setKrmilnik(RED_MODE_7);
  
  prevbuttstate = currbuttstate = 0;
  preMode = curMode = MODE_OFF;
  modeCnt = 200; light = 0;
  attachInterrupt(digitalPinToInterrupt(PIN_IN_RIGHT), blinkRight, FALLING); //LOW, CHANGE, RISING, FALLING
  attachInterrupt(digitalPinToInterrupt(PIN_IN_LEFT),  blinkLeft,  FALLING); //LOW, CHANGE, RISING, FALLING
  //inhvlcd5 = 100; 
  ivlcd5ka = VLCD5_T_KA;
  // if (0 and digitalRead(PIN_VLCD5_ISON_A1 == 0)) { // if VLCD5 is ON then execute the condition
  //   digitalWrite(PIN_LED_INTERNAL, HIGH); // *** ON  INTERNAL_LED ***
  //   if (DBG) Serial.println("PIN_VLCD5_PWR_11 -> VLCD5_POFF  -> HIGH ");
  //   PinPulse(PIN_VLCD5_PWR_11,  HIGH, VLCD5_POFF, VLCD5_PIDLE);  // POWER OFF
  //   digitalWrite(PIN_LED_INTERNAL, LOW);  // *** OFF INTERNAL_LED ***
  //   if (DBG) Serial.println("PIN_VLCD5_PWR_11 -> VLCD5_POFF  -> LOW -> POWER OFF ");
  //   inhvlcd5 = 10;
  // }
  melodyMillis = 0;
  interrupts();         // enable all interrupts

  cli_.SetDefaultHandler(cmd_unrecognized);
  cli_.AddCommand(&cmd_note_);
  cli_.AddCommand(&cmd_set_dur_);
  
  button.setPressedHandler(pressed);
  button.setReleasedHandler(released);

  Serial.println("watchdog Demo Starting");
  wdt_disable(); // Disable the watchdog and wait more 2 second 
  delay(100);   // Include so that Arduino doesn't keep reseting infinitely in case of wrong configuration
  wdt_enable(WDTO_1S); // Enable the watchdog with a timeout of 2 seconds
  //delay(1100);
  Serial.println("Ready!");
}
/***********************  MAIN LOOP  repeats forever ******************************/
void loop() {
  //if(DBG) Serial.print(".");
  wdt_reset();
  interrupts();
  currentMillis = millis();
  cli_.ReadSerial();
  button.loop(); // MUST call the loop() function first
  if (bAssistDNgo) {bAssistDNgo = false; assistDN();}
  if (bAssistUPgo) {bAssistUPgo = false; assistUP();}

  if(button.isPressed()) {
    if(button_Millis) {
      if ((currentMillis - button_Millis) > LONGPRESS) {
        switch (iAssistLevelNow) {
          case 3:
            button_Millis = 0;
            longpress(button);
        }
      }
    } 
  }
  switch (curMode) {
    case MODE_RIGHT:
      noInterrupts();
      if(digitalRead(PIN_IN_RIGHT) == LOW) modeCnt = BLINK_INH_VAL; //extend 
      if(preMode != MODE_RIGHT) {
        setKrmilnik( RED_MODE_OFF );   //2 blink all
        preMode = curMode; 
        if(DBG) Serial.println("MODE CHANGE -> MODE_RIGHT");
        StripLR.prevMillis = 0;
        StripLR.ledPwr = LOW;
      }
      StripLR.Update(currentMillis);
      if(StripLR.change) {
        Serial.print('>');
        LedsROff();
        LEDsAssist();
        //if (!btn10_ison) tone(PIN_BUZZ_OUT, BUZZ_FREQ, BUZZ_DUR);  // delay(BUZZ_DUR); no more active
        if(StripLR.ledPwr) {
          SetLeds(1,LP_BFR,   16+37+16, yellow); //Led Pins zadaj-spredaj-desno 

          SetLeds(1,LP_FRR ,22, yellow);         //Led Pins spredaj-desno-desno 
          //SetLeds(1,LP_FRR,  1, cyan); //TEST 
          strip.show();
        }
        StripLR.change = 0;  //all done no need to change anything 
      }
      if (!modeCnt) {curMode = MODE_NORM; modeCnt = 200; }  // switch mode if zero decrament by TIMER1 int
      break;
    case MODE_LEFT:
      noInterrupts();
      if(digitalRead(PIN_IN_LEFT) == LOW) modeCnt = BLINK_INH_VAL; //extend
      if(preMode != MODE_LEFT) {
        setKrmilnik( RED_MODE_OFF );   //2 blink all
        preMode = curMode; 
        if(DBG)Serial.println("MODE CHANGE -> MODE_LEFT");
        StripLR.prevMillis = 0;
        StripLR.ledPwr = LOW;
      }
      StripLR.Update(currentMillis);
      if(StripLR.change) {
        Serial.print('<');
        LedsLOff();
        LEDsAssist();
        //if (!btn10_ison) tone(PIN_BUZZ_OUT, BUZZ_FREQ, BUZZ_DUR); // delay(BUZZ_DUR); no more active
        if(StripLR.ledPwr) {
          SetLeds(1,LP_BM  , 16+37+16, yellow); //Led Pin Zadaj Levo 

          SetLeds(1,LP_FM+1,22, yellow);
          //SetLeds(1, LP_FM+1,  1, cyan); //TEST 
          strip.show();
        }
        StripLR.change = 0;  //all done no need to change anything
      }
      if (!modeCnt) {curMode = MODE_NORM;  modeCnt = 200; }  // switch mode if zero decrament by TIMER1 int
      break;
    case MODE_NORM:
      noInterrupts();
      if(preMode != MODE_NORM) {
        setKrmilnik( RED_MODE_7 );   //blink scan
        preMode = curMode; 
        if(DBG) Serial.println("MODE CHANGE -> MODE_NORMAL");
        StripNormal.prevMillis = 0;
        StripLR.ledPwr = StripNormal.ledPwr = LOW;
      }
      //Serial.print('!');
      if(digitalRead(PIN_IN_RIGHT) == LOW) curMode = MODE_RIGHT; //start 
      if(digitalRead(PIN_IN_LEFT ) == LOW) curMode = MODE_LEFT; //start 

    /*StripNormal.Update(currentMillis);
      if(StripNormal.change) {
        //Serial.print('!');
        if(digitalRead(PIN_IN_RIGHT) == LOW) curMode = MODE_RIGHT; //start 
        if(digitalRead(PIN_IN_LEFT ) == LOW) curMode = MODE_LEFT; //start 
        LedsOff();      
        if(StripNormal.ledPwr and !btn10_ison)  {
          //Serial.print("-");
          // if (!light) {
          //   for ( uint8_t i =   0; i < 39; i++) { strip.setPixelColor(i, dark);}
          //   for ( uint8_t i =  21; i < 24; i++) { strip.setPixelColor(i, dark);}
          //   for ( uint8_t i =  24; i < 39; i++) { strip.setPixelColor(i, dark);}
          // }
          //
          SetLeds(1, LP_BFR, 15, white);
          //SetLeds(1, LP_BRF+0, 2, whiteL); SetLeds(1,  LP_BRF+2, 2, blueL); SetLeds(1,  LP_BRF+4, 2, redL);  //RIGHT SI FLAG
          //SetLeds(1, LP_BRB+0, 2, whiteL); SetLeds(1,  LP_BRB+2, 2, blueL); SetLeds(1,  LP_BRB+4, 2, redL);  //RIGHT SI FLAG
          SetLeds(1, LP_BM-14  , 12, red);
          //SetLeds(1, LP_BM  ,  1, cyan); //TEST
          SetLeds(1, LP_BM+5   , 12, red);
          //SetLeds(1, LP_BM+5  ,  1, cyan); //TEST
          SetLeds(1, LP_BFL , 15, white);
          //SetLeds(1, LP_BFL,  1, cyan); //TEST

          SetLeds(1, LP_FM-5 , 11, white);
          SetLeds(1, LP_FM ,  1, cyan); //TEST

          if (!(iAL_cnt % 5)) iAL = !iAL;

          iAL_cnt++;
          if (light) { 
           if(modeCnt < 100) modeCnt++;     //never expire!!!!
            SetLeds(1, LP_BFR  , 6, white);
            SetLeds(1, LP_BFL  , 6, white);
            SetLeds(1, LP_FFST , 8, white);
            //for ( uint8_t i =  LP_BFR; i < LP_BFR+3; i++) { strip.setPixelColor(i, white);}
            //for ( uint8_t i =  LP_BFL; i < LP_BFL+3; i++) { strip.setPixelColor(i, white);}
            //for ( uint8_t i =  LP_FM; i < LP_FM+3; i++) { strip.setPixelColor(i, white);}
          }
          LEDsAssist();
          strip.show();
        }
        StripNormal.change = 0;  //all done no need to change anything
      }
    */    
      modeCnt = 2000;
      break;
    case MODE_OFF:
      noInterrupts();    
      switch (vlcd5Mode) {    // bloking function "PinPulse" !!!
        case VLCD5_M_UNKNOW:  // initialize over power cycle
          if(DBG) Serial.println("VLCD5 MODE -> UNKNOW"); 
          iwdt = CNT_INH_WDT_RST;
          //check if power is ON, if yes first POWER OFF
          digitalWrite(PIN_LED_INTERNAL, HIGH); // *** ON  INTERNAL_LED **
          if (digitalRead(PIN_VLCD5_ISON_A1) == 0) {   // if VLCD5 is ON then first POWER OFF
            if (VLCD5_LET_ON) {
              if(DBG) Serial.println("VLCD5 MODE -> UNKNOW: POWER LET IT ON");
              vlcd5Mode = VLCD5_M_DONE;
              iAssistLevelNow  = 4; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO 
              iAssistLevelWant = 2; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO
              bAssistLevelViaL = true;
              break; // exit case VLCD5_M_UNKNOW
            }
            if(DBG) Serial.println("VLCD5 MODE -> UNKNOW: POWER OFF");
            PinPulse(PIN_VLCD5_PWR_11, HIGH, VLCD5_T_POFF, VLCD5_T_PIDLE);  // first pulse POWER OFF VLCD5
          }
          //check if power is OFF, then POWER ON
          if (digitalRead(PIN_VLCD5_ISON_A1) == 1) { 
            if(DBG) Serial.println("VLCD5 MODE -> UNKNOW: POWER ON -> ECO");
            PinPulse(PIN_VLCD5_PWR_11, HIGH, VLCD5_T_PULSE, VLCD5_T_PWRIDLE);  // first pulse POWER ON VLCD5
          }
          // check if POWER is ON then change mode to VLCD5_M_POWER
          if (digitalRead(PIN_VLCD5_ISON_A1) == 0) {
            vlcd5Mode = VLCD5_M_DISPLAY;
          }else{
            if(DBG) Serial.println("VLCD5 MODE -> CAN'T HELP CHECK BATTERY");
            vlcd5Mode = VLCD5_M_DONE; // cant help so set as DONE
            iAssistLevelNow  = 2; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO 
            iAssistLevelWant = 2; // 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO              
          }
          break;
        case VLCD5_M_DISPLAY: // SET DISPLAY
          PinPulse(PIN_VLCD5_PWR_11, HIGH, VLCD5_T_PULSE, VLCD5_T_PIDLE);     // this pulse switch DISPLAY LIGHT ON
          if(DBG) Serial.println("VLCD5 MODE -> DISPLAY: LIGTH ON");
          vlcd5Mode = VLCD5_M_ASSIST;
          break;
        case VLCD5_M_ASSIST:  // SET ASSIST 
          assistUP();
          if(DBG) Serial.println("VLCD5 MODE -> ASSIST: INCREASE BY ONE -> TOUR");
          vlcd5Mode = VLCD5_M_DONE;
          break;
        case VLCD5_M_DONE:    // update internal LED, ...
          digitalWrite(PIN_LED_INTERNAL, LOW);  // *** OFF INTERNAL_LED ***
          curMode = MODE_NORM;
          vlcd5Mode = VLCD5_M_KA;
          break;
      }
      break;
      interrupts();         // enable all interrupts
  }

  if (vlcd5Mode == VLCD5_M_KA) {  // this must be like standalone checking
    noInterrupts();
    if (ivlcd5ka == 0) {                    // if zero - KA is neded to periodicaly extend the POWER ON state
      digitalWrite(PIN_LED_INTERNAL, HIGH); // *** ON  INTERNAL_LED ***
      assistDN();
      if (DBG) Serial.println("VLCD5 MODE -> KA: DN/UP");
      digitalWrite(PIN_LED_INTERNAL, LOW);  // *** OFF INTERNAL_LED ***
      delay(50);
      digitalWrite(PIN_LED_INTERNAL, HIGH); // *** ON  INTERNAL_LED ***
      assistUP();
      digitalWrite(PIN_LED_INTERNAL, LOW);  // *** OFF INTERNAL_LED ***
      ivlcd5ka = VLCD5_T_KA; 
    }
  }

  // put your main code here, to run repeatedly:
  if(f_wdt == 1) {
  //digitalWrite(PIN_LED_INTERNAL, HIGH);  // *** ON  INTERNAL_LED ***
    if(digitalRead(PIN_PWR_STRIP)){strip.show();}
    if (vlcd5Mode != VLCD5_M_KA) {
      setKrmilnik( RED_MODE_OFF );   //2 blink all
    }
  
    if (!DBG) { 
      /***************** Re-enter sleep mode. *****************/
      f_wdt = 0;    /* Don't forget to clear the flag. */
      enterSleep();
    }
  } else {
    /* Do nothing. */
  }  
  //Serial.print('.');
  interrupts();         /* enable all interrupts */
  delay(10);
}

/////////////////////////////////////////////////////////////////
// 0-OFF, 1-ECO 2-TOUR, 3-SPEED, 4-TURBO 
void pressed(Button2& btn) {
  if (btn == button) {
    button_Millis = currentMillis;
    switch(iAssistLevelNow) {
      case 2:
        iAssistCnt = ASSIST_TOUT;
        iAssistLevelWant = 3;
        break;
      case 3:
        iAssistLevelWant = 2; 
        break;
      case 4:
        iAssistLevelWant = 3; 
        break;
    }
    timer1_1s = 1;
  } 
}
void longpress(Button2& btn) {
    if (btn == button) {
      Serial.print("L");
      switch(iAssistLevelNow){
        case 3:
          if (bAssistLevelDir == 1) {
            iAssistLevelWant = 4;
          } else {
            iAssistLevelWant = 2;
          }
         break;
      }
      //timer1_1s = 2;
    }
}
void released(Button2& btn) {
  if (btn == button) {
      button_Millis = 0;
  }
}
/////////////////////////////////////////////////////////////////
void assistUP() {
  //Serial.println("ENTER assistUP!!!");
  ivlcd5ka = VLCD5_T_KA;
  bAssistLevelDir = 1;
  PinPulse(PIN_VLCD5_UP_12,  HIGH, VLCD5_T_LPULSE, VLCD5_T_PIDLE);  // ASSIST UP
  Serial.print("/");
  if (iAssistLevelNow  == 4) { //to be shure that is top assist
    delay(200);
    PinPulse(PIN_VLCD5_UP_12,  HIGH, VLCD5_T_LPULSE, VLCD5_T_PIDLE);  // ASSIST UP
    Serial.print("/");
  }
  //Serial.println("EXIT assistUP!!!");
}
void assistDN() {

  ivlcd5ka = VLCD5_T_KA;
  bAssistLevelDir = 0;
  PinPulse(PIN_VLCD5_DN_A0,  HIGH, VLCD5_T_LPULSE, VLCD5_T_PIDLE);  // ASSIST DOWN
  Serial.print("\\");
}

  /***********************  ISR  ******************************/
ISR(WDT_vect) {         // Interrupt Service Routine:  WATCHDOG!
  noInterrupts();
  if (f_wdt == 0) {
    f_wdt = 1;
    iwdt = CNT_INH_WDT_RST;
  } else {
    if ( iwdt ) {iwdt--;}
    if ( iwdt == 0) {
      Serial.println("WDT Overrun!!!"); delay(20);
      // software_Reset();
    }
  }
  interrupts();
}

ISR(TIMER1_OVF_vect) {  // Interrupt Service Routine:  TIMER1:  0,1ms or 10Hz
  noInterrupts();
  TCNT1 = timer1_counter;    // preload timer
  giWL = 2;
  giOfs = 14;
  switch (giDL) {
    case 0:
      if (giFL)         {giFL--;} else {giDL = 1;}
      break;
    case 1:
      if (giOfs - giFL) {giFL++;} else {giDL = 0;}
      break;
  }
  switch (curMode){
    case MODE_NORM:
      LedsOff();
      gidiv = 3;
      LEDsFly(LP_FM, giOfs/gidiv, giWL, giDL, giFL/gidiv, white);

      LEDsFly(LP_BFRM, giOfs/gidiv, giWL, giDL, giFL/gidiv, white);
      LEDsFly(LP_BFLM, giOfs/gidiv, giWL, giDL, giFL/gidiv, white);
      
      LEDsFly(LP_BRM, giOfs, giWL, giDL, giFL, blue);
      LEDsFly(LP_BLM, giOfs, giWL, giDL, giFL, blue);
      gidiv = 3;
      LEDsFly(LP_FRM, giOfs/gidiv, giWL-1, giDL, giFL/gidiv, blue);
      LEDsFly(LP_FLM, giOfs/gidiv, giWL-1, giDL, giFL/gidiv, blue);
  
      LEDsFly(LP_BBM, giOfs, giWL, giDL, giFL, red);
      LEDsAssist();
      LEDsBrake(giFL);
      strip.show(); 
      break;
    case MODE_LEFT:
      LedsROff();
      gidiv = 3;
      LEDsFly(LP_FRM, giOfs/gidiv, giWL-1, giDL, giFL/gidiv, blue);
      LEDsFly(LP_BRM, giOfs, giWL, giDL, giFL, blue);
      LEDsAssist();
      LEDsBrake(giFL);
      strip.show(); 
      break;
    case MODE_RIGHT:
      LedsLOff();
      gidiv = 3;
      LEDsFly(LP_FLM, giOfs/gidiv, giWL-1, giDL, giFL/gidiv, blue);
      LEDsFly(LP_BLM, giOfs, giWL, giDL, giFL, blue);
      LEDsAssist();
      LEDsBrake(giFL);
      strip.show(); 
      break;
  }

  if (curMode == MODE_NORM) {
  
  }
  if (timer1_1s) {
    timer1_1s--;
    //Serial.print("!");
  } else {
    timer1_1s = SETT1_0s1;
    if(bAssistLevelViaL == true) {
      if(iAssistLevelNow) {
        iAssistLevelNow--;
        bAssistDNgo = true;
      } else bAssistLevelViaL = false; 
    } else if(bAssistLevelViaH == true) {
      if(iAssistLevelNow < 4) {
        iAssistLevelNow++;
        bAssistUPgo = true;
      } else bAssistLevelViaH = false;  
    } else if((iAssistLevelWant - iAssistLevelNow) > 0) {
      iAssistLevelNow++;
      bAssistUPgo = true;
    } else if((iAssistLevelNow - iAssistLevelWant) > 0) {
      iAssistLevelNow--;
      bAssistDNgo = true;
    } 
  
    switch (iAssistLevelNow){
      case 2:
        break;
      default:
        if(iAssistCnt){
          iAssistCnt--;
        } else {
          iAssistLevelWant = 2;
        }
        break;
    }
  }
  //digitalWrite(ledPin, digitalRead(ledPin) ^ 1);
  if(curMode !=  MODE_OFF) { 
    if (modeCnt) modeCnt--;    //count to zero
    if (ivlcd5ka) ivlcd5ka--;  //count to zero
  }
}

void blinkRight() { /* This will bring us back from sleep. */
  if(curMode !=  MODE_OFF) {
    curMode = MODE_RIGHT;  
    modeCnt = BLINK_INH_VAL;
  }
}

void blinkLeft() { /* This will bring us back from sleep. */
  if(curMode !=  MODE_OFF) {
    curMode = MODE_LEFT;
    modeCnt = BLINK_INH_VAL;
    }
}
/*******************  FUNCTIONS *********************/

void LEDsFly(uint8_t Mid,uint8_t Ofs, uint8_t iWL, uint8_t iDL, uint8_t iFL, uint32_t color) {
  //for (uint8_t i = Mid-Ofs-iWL; i <= Mid+Ofs+iWL; i++) strip.setPixelColor(i, strip.Color(0, 0, 0));
  for (uint8_t i = Mid-iFL-iWL; i <= Mid-iFL+iWL; i++) strip.setPixelColor(i, color);
  for (uint8_t i = Mid+iFL-iWL; i <= Mid+iFL+iWL; i++) strip.setPixelColor(i, color);
}

void LEDsAssist() {
  switch(iAssistLevelNow){
    case 3:
      SetLeds(1,LP_BM-10 ,1, orange);
      SetLeds(1,LP_BM+10 ,1, orange);
      SetLeds(1,LP_FFST  ,1, orange);
      SetLeds(1,LP_FLST-1  ,1, orange);
      SetLeds(1,LP_FM-4  ,1, cyan);
      SetLeds(1,LP_FM+4  ,1, cyan);
      break;
    case 4:
      SetLeds(1,LP_BM-10 ,2, orange);
      SetLeds(1,LP_BM+10 ,2, orange);
      SetLeds(1,LP_FFST  ,2, orange);
      SetLeds(1,LP_FLST-2,2, orange);
      SetLeds(1,LP_FM-4  ,2, cyan);
      SetLeds(1,LP_FM+4  ,2, cyan);
      break;
  }
}
void LEDsBrake(uint8_t iFL) {
  uint8_t const iand = 2;
  if (digitalRead(PIN_IN_BRAKE)==0) {
    switch(iFL & iand ){
      case iand:
        SetLeds(1,LP_BM-5 ,10, blue);
        break;
      default:
        SetLeds(1,LP_BM-5 ,10, red);
        break;
    }
  }
}

void software_Reset() { // Restarts program from beginning but does not reset the peripherals and registers
  asm volatile ("  jmp 0");  
}  

void StripOff() {
  if(digitalRead(PIN_PWR_STRIP)) {
    digitalWrite(PIN_DOUT, HIGH); // becouse NPN transistor  
    digitalWrite(PIN_PWR_STRIP, LOW);
  }  /* POWER STRIP LED OFF */
}

void PinPulse(uint8_t pin, bool value, uint16_t dur_ms, uint16_t idle_ms) {
  digitalWrite(pin, value);  delay(dur_ms);
  digitalWrite(pin, !value); delay(idle_ms);
}

void LedsOff() {
  strip.clear();
}
void LedsLOff() {
  for(uint8_t i=LP_BM;   i<LP_BLST; i++ ){strip.setPixelColor(i, strip.Color(0, 0, 0));}
  for(uint8_t i=LP_FM;   i<LP_FLST; i++ ){strip.setPixelColor(i, strip.Color(0, 0, 0));}
}
void LedsROff() {
  for(uint8_t i=0;       i<LP_BM;   i++ ){strip.setPixelColor(i, strip.Color(0, 0, 0));}
  for(uint8_t i=LP_FFST; i<LP_FM;   i++ ){strip.setPixelColor(i, strip.Color(0, 0, 0));}
}


void SetLeds(uint8_t state, uint8_t pos, uint8_t len, uint32_t color) {
  switch (state) {
    case 0:
      for(uint8_t i=pos; i<pos+len; i++ ){strip.setPixelColor(i, strip.Color(0, 0, 0));}
      break;
    case 1:
      for(uint8_t i=pos; i<pos+len; i++ ){strip.setPixelColor(i, color);}
      break;    
  }
}

void enterSleep(void) {
  set_sleep_mode(SLEEP_MODE_PWR_SAVE);   /* EDIT: could also use SLEEP_MODE_PWR_DOWN for lowest power consumption. */
  sleep_enable();
  sleep_mode();    /* Now enter sleep mode. */
  /* The program will continue from here after the WDT timeout*/
  sleep_disable(); /* First thing to do is disable sleep. */
  /* Re-enable the peripherals. */
  power_all_enable();
}