/* 2024 02 22 19:00
   
   versione con comando >down< longclick 
   derivante dalla 007

   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>  
#include <SPI.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,20,4);  // 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 DWC 5  // Down
#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 HZD 15  // Hazard in pullup


#define DEBUG 0

/* 
  look well  look well  look well  look well  look well 
  se sei su wokwi devi commentare la riga 18 con #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 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
int btnhzd = digitalRead(7);

// light
#define RELDX 9
#define RELSX 8

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

// blink
bool flag_sx,flag_dx;
unsigned long clock, time,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");
  #endif
  
  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...
// CAN0.init_Filt(5,0, 0x01070000);                // Init sixth filter...
 
  pinMode(INPUT, INPUT);     // Setting pin 2 for /INT input
  pinMode(7, INPUT_PULLUP);
  pinMode(RELDX, OUTPUT);
  pinMode(RELSX, 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();
  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(12, 3);
  lcd.print("10:12:23");

  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; 
  L_old_time=0;

  lcd.setCursor(4, 1);  lcd.print("  WELCOME  ");
  lcd.setCursor(4, 2);  lcd.print(" in  MKSTM ");
  command=WTC; old_command=NTD;
}

bool live=false;

void loop()
{  clock=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)
  { AS_time=clock;
    if( (AS_time-AS_old_time) > AS_time_elapsed)
    { command=STC; old_command=NTD;
      lcd.setCursor(4, 2); 
      lcd.print(" HB end    ");
      // delay(300); // serve?
  } }

  //#if LONG_PRESSED 
  // long time timeout  
  // funziona quasi come un timer del MCU quindi deve rimanere fuori 
  // dallo switch
  if(full_blink_status==1 )
  { L_time=clock;
    if ( (L_time-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; 
        command=WTC; old_command=NTD; // come in setup
        delay(200); // bello, ma serve????
        lcd.setCursor(4, 1); lcd.print("           .");   
  } }
  //#endif

       // status half blink
      if (flag_sx) 
      { time=clock;   
        if((time-old_time)>blink_semiperiod)
        { old_time=time;
          digitalWrite(RELSX, !digitalRead(RELSX));
      } }
      if (old_command==SXC)  old_command==NTD; 
      

      if (flag_dx)
      { time=clock; 
        if((time-old_time)>blink_semiperiod)
        { old_time=time;
          digitalWrite(RELDX, !digitalRead(RELDX));  
      } }
      if (old_command==DXC) 
      { old_command==NTD; 
      }
     
      //#if LONGPRESSED
      if (full_blink_status==2)
      { time=clock;   
        if((time-old_time)>blink_semiperiod)
        { old_time=time;
          digitalWrite(RELSX, !digitalRead(RELSX));
          digitalWrite(RELDX, !digitalRead(RELDX));  
      } }
      //#endif


  // WAIT COMMAND
  #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=DWC;
    } 
  }
  if (command==DWC && old_command==NTD) 
  { old_command=FDW;
    L_old_time=millis();
  }
  #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(tmp_command!=0) command=tmp_command;    
    if(tmp_command==DWC)  
    { 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 DEBUG
    lcd.setCursor(1, 0); 
    lcd.print("01 ");
    lcd.print(command);
    lcd.print(" ");
    lcd.print(old_command);
    lcd.print("     ");
    #endif
  }  
  #endif

  if (command==DWC && old_command==FDW) LP_time=millis()-L_old_time;
  if (command!=DWC) 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);
      flag_sx=HIGH;
      flag_dx=LOW;
      autostop_flag=HIGH;
      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);
      flag_dx=HIGH;
      flag_sx=LOW;
      autostop_flag=HIGH;
      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
      flag_sx=LOW;
      flag_dx=LOW;

      old_time=0;
      AS_old_time=0;
      autostop_flag=LOW;

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

      delay(100); // necessario

      break;

      case DWC:  

      // DW è stato premuto arriva la prima raffica
      if(full_blink_status==0)
      { 
        lcd.setCursor(4, 1); lcd.print("            "); 
        lcd.setCursor(4, 2); lcd.print(" DW Pressed ");
        full_blink_status=1;
      }  
     
      if(LP_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=DWC;  
 
      break;
	  
	  case HZD: // Premuto Hazard in input_pullup
	  #if DEBUG
	    lcd.setCursor(1, 0); lcd.print("07")
    #endif

      if(digitalRead(7) == HIGH)
		 
      { 
        full_blink_status==0;
        lcd.setCursor(4, 1); lcd.print("            "); 
        lcd.setCursor(4, 2); lcd.print("   HAZARD   ");
        //full_blink_status=1;
     
          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=DWC;  
 
	    break;
	  

      case LDC: 
      break;

      case WTC:
      #if 0
        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;
    }
  }
}