/* 2024 03 26 12_09
   
   versione con comando >double arrows< affidato a C1 e PIN7
   
   l'opzione viene impostata con questi due parametri 
   #define timeout 3000
   #define LongPressed 1000 


   NOTA per tutti i comandi  
   
   un click si considera eseguito quando viene lasciato il pulsante 
   o quando cambia il comando obd

   NOTA per l'autore del codice
   nel codice vanno ripuliti e gestiti meglio gli old_command
*/
#include <mcp_can.h>  
// by Coryjflower
#include <SPI.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,20,4);  // by Marco Schwartz set the LCD address to 0x27 for a 16 chars and 2 line display

#define SXC 1 
#define DXC 2 
#define HBK 3  // half blink
#define STC 4  // Stop 
#define DAC 5  // Double Arrows
#define LDC 6  // DOWN prolungato
#define FBK 7  // full blink
#define WTC 8  // Wait command
#define NTD 11 // not defined
#define DEF 12 // defined
#define FDW 14 // first_down

#define DEBUG 0

/* 
  look well  look well  look well  look well  look well 
  se sei su wokwi devi commentare la riga #define MCP2515 
  esempio: // #define MCP2515
  se stai usando la scheda MCP2515 allora non la devi commentare
  esempio:    #define MCP2515
*/
//#define MCP2515   

//buffer
long unsigned int rxId;   
unsigned char len = 0;
unsigned char rxBuf[8]; 

// MCP CS on SPI SS
MCP_CAN CAN0(10);   

// INPUT
#define INPUT 2
   const byte data[8] = {0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};

   const byte btndx[] = {0x04, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando freccia destra
   const byte btnsx[] = {0x10, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando freccia sinistra
   const byte btnce[] = {0x08, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando centro disattiva frecce
   const byte btndn[] = {0x02, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando C1
// const byte btndn[] = {0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando C2 ???
// const byte btndn[] = {0x00, 0x00, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00}; //comando 4 frecce

// light
#define RELDX 9
#define RELSX 8

int command, old_command, tmp_command;  // command from OBD or switches

// blink
bool flag_sx,flag_dx,flag6;
unsigned long mclock, old_time;                  
#define blink_semiperiod 400                   

// autostop
bool autostop_flag;                        
unsigned long AS_time,AS_old_time; 
#define AS_time_elapsed 25000 // auto spegnimento half blink

// full blink
int full_blink_status; // 0 off, 1 full blin waiting , 2 full blink ON
unsigned long L_time,L_old_time, LP_time; 
#define timeout 2000
#define LongPressed 1000 

void setup()
{
  Serial.begin(115200);
  #ifdef MCP2515
  if(CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_8MHZ) == CAN_OK) Serial.print("MCP2515 Init Okay!!\r\n");
  else Serial.print("MCP2515 Init Failed!!\r\n");
    
  // CAN0.resetta();                             // fa impallare ECU
  CAN0.init_Mask(0,0,0x07FFF000);                // Init first mask...
  CAN0.init_Filt(0,0,0x01401000);                // Init first filter...
  CAN0.init_Filt(1,0,0x01400200);                // Init second filter...
  
  CAN0.init_Mask(1,0,0x07FFF000);                // Init second mask... 
  CAN0.init_Filt(2,0,0x01400400);                // Init third filter...
  CAN0.init_Filt(3,0,0x01400800);                // Init fourth filter...
  CAN0.init_Filt(4,0,0x01400100);                // Init fifth filter... x C2
  // CAN0.init_Filt(5,0, 0x01070000);                // Init sixth filter...
  #endif

  pinMode(INPUT, INPUT);        // Setting pin 2 for /INT input
  pinMode(7, INPUT_PULLUP);     // Setting pin 2 for /INT input

  pinMode(RELDX, OUTPUT);
  pinMode(RELSX, OUTPUT);
  pinMode(    6, OUTPUT);
  digitalWrite(RELDX, HIGH);  //rele su spento
  digitalWrite(RELSX, HIGH);  // rele su spento

  CAN0.setMode(MCP_NORMAL);   // Change to normal mode to allow messages to be transmitted
  
  lcd.init();
  lcd.backlight();

  digitalWrite(RELDX, HIGH);
  digitalWrite(RELSX, HIGH);
  flag_dx=LOW;
  flag_sx=LOW;

  autostop_flag=LOW;
  AS_old_time=0;
  
  old_time=0;
  full_blink_status=0; 
  LP_time=0;
  L_old_time=0;
  flag6=false;

  lcd.setCursor(0, 1);  lcd.print(" Turn Light System");
  lcd.setCursor(0, 2);  lcd.print("KTM  with Akracovic");
  delay(1500);

  lcd.clear();
  lcd.setCursor(18, 0);
  lcd.print("ON");
  lcd.setCursor(0, 1);
  lcd.print("MSG:");
  lcd.setCursor(0, 2);
  lcd.print("LST:");
  lcd.setCursor(6, 1);  lcd.print("System");
  lcd.setCursor(6, 2);  lcd.print(" Ready");
  //lcd.setCursor(12, 3); lcd.print("10:12:23");
  command=WTC; old_command=NTD;
}

bool live=false;

int pin7read; //Lettura corrente del pin INPUT PULSANTE
int pin7previous = HIGH; //Lettura precedente del pin INPUT PULSANTE
int pin7firsttime=false;
long pin7lasttime = 0; //Ultimo tempo in cui il pin di OUTPUT è stato attivato

void loop()
{  mclock=millis();

   #if DEBUG
    lcd.setCursor(0, 0); 
    live=!live;
    if(live) lcd.print("*");
    else     lcd.print(" ");
   #endif

  // lcd.print(" ");lcd.print(command);lcd.print(" ");lcd.print(old_command);
  // autostop del simpleblinking
  // funziona quasi come un timer del MCU quindi deve rimanere fuori 
  // dallo switch
  if(autostop_flag)
  { if( (mclock-AS_old_time) > AS_time_elapsed)
    { command=STC; old_command=NTD;
      lcd.setCursor(4, 2); 
      lcd.print(" HB end    ");
      for (int i=0;i<10;i++)
      { CAN0.sendMsgBuf(0x140, 0, 8, data);
         delay(50);
      }
  } }

  // LONG_PRESSED 
  // long time timeout  
  // funziona quasi come un timer del MCU quindi deve rimanere fuori 
  // dallo switch
  if(full_blink_status==1 )
  { if ( (mclock-L_old_time) > timeout ) 
    {   // 300 va calibrato se DW viene alzato presto
        // status era stato premuto, ma è passato troppo tempo
        lcd.setCursor(4, 1); lcd.print("  Time out  ");
        lcd.setCursor(4, 2); lcd.print("    STOP   ."); 
        full_blink_status=0; 
        L_old_time=mclock;
        command=WTC; old_command=NTD; // come in setup
        delay(200); // bello, ma serve????
        lcd.setCursor(4, 1); lcd.print("           .");   
  } }

    // status half blink
  if (flag_sx) 
  { if((mclock-old_time)>blink_semiperiod)
    { old_time=mclock;
      digitalWrite(RELSX, !digitalRead(RELSX));
  } }
  if (old_command==SXC)  old_command==NTD; 
  
  if (flag_dx)
  { if((mclock-old_time)>blink_semiperiod)
    { old_time=mclock;
      digitalWrite(RELDX, !digitalRead(RELDX));  
  } }
  if (old_command==DXC) 
  { old_command==NTD; 
  }
  
  // LongPressed
  if (full_blink_status==2)
  { if((mclock-old_time)>blink_semiperiod)
    { old_time=mclock;
      digitalWrite(RELSX, !digitalRead(RELSX));
      digitalWrite(RELDX, !digitalRead(RELDX));  
  } }

  // WAIT COMMAND

  /* int pin7read; //Lettura corrente del pin INPUT PULSANTE
     int pin7previous = HIGH; //Lettura precedente del pin INPUT PULSANTE
     int pin7firsttime=false;
     long pin7lasttime = 0; //Ultimo tempo in cui il pin di OUTPUT è stato attivato
  */

  // pin6 toggle with pin7
  #if 0
  pin7read = digitalRead(7); //Leggo il valore in ingresso dato dal pulsante
  if (pin7read == LOW)
  { if (pin7previous==HIGH)
    {  if( !pin7firsttime)
       { pin7firsttime=true;
         pin7lasttime=mclock;
       }
       else
       { if( (mclock-pin7lasttime) > LongPressed )
         { flag6=!flag6;digitalWrite(6,flag6);
           pin7lasttime = mclock;
           pin7previous=LOW;
         }
       }  
    }
  }  
  else 
  { pin7firsttime=false;
    pin7previous=HIGH;
  } 
  #endif

  // pin6 toggle with pin7
 
  pin7read = digitalRead(7); //Leggo il valore in ingresso dato dal pulsante
  if (pin7read == LOW && full_blink_status==0)
  { if (pin7previous==HIGH)
    { if( !pin7firsttime)
      { pin7firsttime=true;
        pin7lasttime=mclock;
      }
      else
      { if( (mclock-pin7lasttime) > LongPressed )
        { pin7previous=LOW;
          digitalWrite(RELDX, LOW);
          digitalWrite(RELSX, LOW);
          digitalWrite(6, HIGH);
          flag_sx=LOW; 
          flag_dx=LOW;
          old_time=millis();
        
          lcd.setCursor(4, 1); lcd.print("           .");
          lcd.setCursor(4, 2); lcd.print("   Blink   .");   

          autostop_flag=LOW;
          AS_old_time=0;
        
          L_old_time=0; 
          full_blink_status=2; // blink mode
          old_command=NTD;
          command=WTC;   
  } } } }
  else 
  { pin7firsttime=false;
    pin7previous=HIGH;
  } 


/*


*/


  
  
  #ifdef MCP2515

  // attendo comando dalla scheda 2515 
  // se PIN2 viene portato a zero 
  // prelievo il dato da SPI 
  if(!digitalRead(2)) CAN0.readMsgBuf(&rxId, &len, rxBuf); 
  { if(len==8)      
    {                                 
      if (memcmp(btnsx,rxBuf,len)==0) command=SXC; 
      if (memcmp(btndx,rxBuf,len)==0) command=DXC; 
      if (memcmp(btnce,rxBuf,len)==0) command=STC;
      if (memcmp(btndn,rxBuf,len)==0) command=DAC;
    } 
  }

  #if 0
  if(!digitalRead(7)) //2024 03 22
  { delay(200);
    if(!digitalRead(7)) command=DAC;
  }
  #endif
  
  #if 0
  if (command==DAC && old_command==NTD) 
  { old_command=FDW;
    L_old_time=millis();
  }
  #endif

  
  #else
  
  // attendo comando da pulsantiera analogica
  if(command==WTC)
  { tmp_command =analogRead(A0); 
    tmp_command+=analogRead(A1)*2; 
    tmp_command+=analogRead(A2)*4;
    tmp_command+=analogRead(A3)*5; 
    tmp_command=(int)(tmp_command/1023);

    #if 0
    if(!digitalRead(7)) //2024 03 22
    { delay(200);
      if(!digitalRead(7)) tmp_command=DAC;
    }
    #endif

    if(tmp_command!=0) command=tmp_command;    

    if(tmp_command==DAC )  
    { old_command=FDW;
      L_old_time=millis();
    }
    while(tmp_command!=0)
    { tmp_command =analogRead(A0); 
      tmp_command+=analogRead(A1)*2; 
      tmp_command+=analogRead(A2)*4;
      tmp_command+=analogRead(A3)*5; 
      tmp_command=(int)(tmp_command/1023);
      #if 0
      if(!digitalRead(7)) tmp_command=DAC; //2024 03 22
      #endif
    }  

    #if DEBUG
      lcd.setCursor(1, 0); 
      lcd.print("01 ");
      lcd.print(command);
      lcd.print(" ");
      lcd.print(old_command);
      lcd.print("     ");
    #endif
  }  
  #endif

  //if (command==DAC && old_command==FDW) LP_time=millis()-L_old_time;
  if (command!=DAC) old_command=DEF; //???

  if(command!=old_command )
  { 
    #if DEBUG
     lcd.setCursor(1, 0); lcd.print("02"); 
    #endif
    switch (command)
    { case SXC:
      #if DEBUG
       lcd.setCursor(1, 0); lcd.print("03"); 
      #endif
      lcd.setCursor(4, 1); lcd.print("            "); 
      lcd.setCursor(4, 2); lcd.print("    Left   ."); 

      digitalWrite(RELSX, LOW);  
      digitalWrite(RELDX, HIGH);
      digitalWrite(6, LOW);  
      flag_sx=HIGH;
      flag_dx=LOW;
      autostop_flag=HIGH;
      full_blink_status=0;
      old_time=millis();
      AS_old_time=old_time; 
      
      command=WTC; old_command=DEF;
      
      break;

      case DXC:
      #if DEBUG
       lcd.setCursor(1, 0); lcd.print("04"); 
      #endif      
      lcd.setCursor(4, 1); lcd.print("            "); 
      lcd.setCursor(4, 2); lcd.print("   Right   .");

      digitalWrite(RELDX, LOW); 
      digitalWrite(RELSX, HIGH);
      digitalWrite(6, LOW);  
      flag_dx=HIGH;
      flag_sx=LOW;
      autostop_flag=HIGH;
      full_blink_status=0;      
      old_time=millis();
      AS_old_time=old_time;
      
      command=WTC; old_command=DEF;
     
      break;

      case STC:
      #if DEBUG
       lcd.setCursor(1, 0); lcd.print("05"); 
      #endif
      // STOP Command
      lcd.setCursor(4, 1); lcd.print("           .");
      lcd.setCursor(4, 2); lcd.print("    STOP   ."); 

      digitalWrite(RELDX, HIGH);  // rele dx spento
      digitalWrite(RELSX, HIGH);  // rele sx spento
      digitalWrite(6, LOW);  
      flag_sx=LOW;
      flag_dx=LOW;
      
      old_time=0;
      AS_old_time=0;
      autostop_flag=LOW;

      L_old_time=0;  
      flag6=false;   
      full_blink_status =0;
            
      command=WTC;  old_command=STC; 

      for (int i=0;i<10;i++)
      { CAN0.sendMsgBuf(0x140, 0, 8, data);
         delay(50);
      }
      //delay(100); // non più necessario

      break;

      case DAC:  

      // DW è stato premuto arriva la prima raffica
      if(full_blink_status==0)
      { 

        full_blink_status=1;
        L_old_time=mclock;
        lcd.setCursor(4, 1); lcd.print("            "); 
        lcd.setCursor(4, 2); lcd.print(" DA Pressed ");
        delay(100);
      }  
     
      if( (full_blink_status==1) && ((mclock-L_old_time)>LongPressed ) )
      {
        digitalWrite(RELDX, LOW);
        digitalWrite(RELSX, LOW);
        flag_sx=LOW; 
        flag_dx=LOW;
        old_time=millis();
        
        lcd.setCursor(4, 1); lcd.print("           .");
        lcd.setCursor(4, 2); lcd.print("   Blink   .");   

        autostop_flag=LOW;
        AS_old_time=0;
        
        L_old_time=0; 
        full_blink_status=2; // blink mode
        old_command=NTD;

      } 
      command=WTC; //old_command=DAC;   
      break;

      case LDC: 
      break;

      case WTC:
      #if DEBUG
        lcd.setCursor(1, 0); lcd.print("08"); 
        if(old_command!=NTD)
        { lcd.setCursor(4, 1);
          lcd.print(" WAIT COM  .");
        }  
      #endif 
      break;

      default:
      lcd.setCursor(4, 1);
      lcd.print("   ? ? ?   ");
      break;
    }
  }
}