/*
Control and direction buttons
JMA 18.5.2026
ošetřená tlačítka reg C toggle s blokováním lock_C / jeví se  funkční o.k
ošetření tlačítek směru - v základním algoritmu - opravena chybička viz pozn//
*/
#include "Arduino.h"
#include "PCF8575.h"
#include <I2C_scan.h>"

// Set i2c address
PCF8575 pcf8575_20(0x20);
PCF8575 PCF_controls(0x21);         //control buttons
PCF8575 PCF_dir(0x22);              //direction buttons
PCF8575 PCF_lock_control(0x23);     //lock control buttons
PCF8575 PCF_lock_dir(0x24);         //lock direction buttons
PCF8575 PCF_SW(0x25);               //switch Controls output
PCF8575 PCF_DIRS(0x26);             //switch Dirs power output

uint16_t  butt_C;
uint16_t  butt_D;
uint16_t  lock_C;
uint16_t  lock_D;
uint16_t  o_butt_C;

uint8_t  butt_h;              // H buttons
uint8_t  butt_g;              // G buttons
uint8_t  lock_h;
uint8_t  lock_g;
uint8_t  o_butt_h;
uint8_t  o_butt_g;
uint8_t  fl_h;
uint8_t  fl_g;
uint16_t  fl_1;
uint16_t  fl_2;
uint16_t o_switch;
uint16_t Switch;

uint8_t  dir_s;    // Directions - s
uint8_t  dir_l;    // Directions - l
uint8_t  lock_s;
uint8_t  lock_l;
uint8_t  o_dir_s;
uint8_t  o_dir_l;
uint8_t  fl_s;
uint8_t  fl_l;
uint16_t  o_dirs;

uint16_t  CONTROLS = 0xFFFF;         // on RESET init
uint16_t  O_CONTROLS;

uint16_t  DIRS = 0xFFFF;         // on RESET init
uint16_t  O_DIRS;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  I2C_scan();                 // scan I2C bus  included file <I2C_scan.h>

  pcf8575_20.begin();
  PCF_controls.begin();       // control buttons H(HALT) G(GO)
  PCF_dir.begin();           // direction (_s) (_l)
  PCF_lock_control.begin();
  PCF_lock_dir.begin();
  PCF_SW.begin();

  PCF_SW.write16(~CONTROLS);    // on RESET init
  Serial.println("Setup completed");

//delay(5000);
  Serial.println("\n... main loop continued ...\n");
}


void loop()
{
delay(50);
// Serial.println(millis());
  butt_C = ~PCF_controls.read16();
  butt_D = ~PCF_dir.read16();
  lock_C = ~PCF_lock_control.read16();
  lock_D = ~PCF_lock_dir.read16();

  butt_h = lowByte(butt_C);   // low Byte Controls - HALT
  butt_g = highByte(butt_C);  // high Byte Controls - GO
  lock_h = lowByte(lock_C);
  lock_g = highByte(lock_C);

  dir_s = lowByte(butt_D);   // low Byte Directions - s
  dir_l = highByte(butt_D);  // high Byte Directions - l
  lock_s = lowByte(lock_D);
  lock_l = highByte(lock_D);

//  fl_h = (butt_h ^ o_butt_h) & ~butt_h & lock_h;
//  fl_g = (butt_g ^ o_butt_g) & ~butt_g & lock_g;

/********************************* nove **************************************/
//  fl_1 = (butt_h ^ o_butt_h) & ~butt_h;
  fl_1 = (butt_C ^ o_butt_C) & ~butt_C  & ~lock_C;    // lock_C ve stavu "0" blokuje toggle
  fl_2 = o_switch ^ fl_1;
  Switch = (~o_switch & fl_1) | fl_2;

/*
//  o_butt_h = butt_h;
//  o_butt_g = butt_g;

  o_switch = (o_switch & (~(o_switch & fl_h) & ~(o_switch & fl_g<<8)));
  o_switch = o_switch | (butt_h | (butt_g<<8));

  CONTROLS = ~o_switch;
*/
/***************************************************************/
  o_butt_h = butt_h;
  o_butt_C = butt_C;
  o_switch = Switch;

  CONTROLS = ~Switch;
  if(CONTROLS != O_CONTROLS) {
    PCF_SW.write16(CONTROLS);
    O_CONTROLS = CONTROLS;
  }
/****************** toggle se jeví funkční ***********************/

// tady pokračovat s ošetřením dir s,l
/*
prozatím připustím současnost aktivní dir_s i dir_l (L298 si to zvládne)
dir funguje jako toggle (pamatuje si)
dir_s by měl schodit dir_l a naopak, 
lock směru by měl fungovat tak že zakazuje (zvenčí) sepnutí výstupu směru
*/
  fl_s = (dir_s ^ o_dir_s) & ~dir_s & lock_s;     // tady jsem mel chybicku lock_g  !!!
  fl_l = (dir_l ^ o_dir_l) & ~dir_l & lock_l;     // tady jsem mel chybicku lock_h  !!!
  o_dir_s = dir_s;
  o_dir_l = dir_l;

  o_dirs = (o_dirs & (~(o_dirs & fl_s) & ~(o_dirs & fl_l<<8)));
  o_dirs = o_dirs | (dir_s | (dir_l<<8));

  DIRS = ~o_dirs;
  if(DIRS != O_DIRS) {
    PCF_DIRS.write16(DIRS);
    O_DIRS = DIRS;
  }

// Serial.println(millis());
  delay(100);
}