/*
 Rocrail - Model Railroad Software
 Copyright (c) 2002-2022 Robert Jan Versluis, Rocrail.net
 All rights reserved.
*/
#include "gpio.h"
#include "cbus.h"
#include "mbus.h"
#include "trace.h"

//---------- CBusIO contructor to init the board.
GPIO::GPIO(byte protocol, byte opt) {
  trace("GPIO board, opt=%d", opt);

  m_SoD      = false;
  m_Ack      = false;
  m_Publish  = true;
  m_Servos   = NULL;

  m_CANID = CV::get(EE_CANID);

  if( m_CANID == 0 ) {
    m_CANID = DEFAULT_CANID;
    trace("Set CANID to %u", m_CANID);
    CV::set(EE_CANID, m_CANID );
  }

  byte l_BoardType = MTYP_CANGC2a;

  #ifdef ARDUINO_ARCH_RP2040
    l_BoardType = MTYP_CANGC2p;
  #endif

  if( protocol == PROTOCOL_MBUS )
    m_Prot = new MBus(m_CANID, l_BoardType);
  else
    m_Prot = new CBus(m_CANID, l_BoardType);
  
  m_ScanIdx = 0;
  while (millis() < 100);
  m_IO = new IO();

  m_IO->initPorts(false);

#ifdef ARDUINO_ARCH_RP2040
  m_Servos = new Servos();
#else
  if( opt & BOARD_OPT_SERVO ) {
    // Init servos...
    m_Servos = new Servos();
  }
#endif

  m_IO->initIO(MAX_PORTS);
  
  m_LastMillis = millis();
  restorePortState();

}


unsigned int GPIO::getCANBPS() {
  return m_Prot->getCANBPS();
}

byte GPIO::getFilters(unsigned long* filters) {
  return m_Prot->getFilters(filters);
}

//---------- Set the SoD flag.
void GPIO::SoD() {
  m_SoD = true;
}


//---------- Set the publish flag which will trigger an OPC_PNN CAN message.
void GPIO::publish() {
  m_Publish = true;
}


void GPIO::setPort(byte port, byte conf) {
  if( port < MAX_PORTS ) {
    if( conf == 0 )
      CV::set(EE_PORTCFG + port, PORTCFG_OUT);
    else if( conf == 1 )
      CV::set(EE_PORTCFG + port, PORTCFG_IN);
    else if( conf == 2 )
      CV::set(EE_PORTCFG + port, PORTCFG_IN | PORTCFG_OFFDELAY );
    else if( conf == 3 )
      CV::set(EE_PORTCFG + port, PORTCFG_OUT | PORTCFG_PULSE );
    m_IO->initIO(port);
  }
}

void GPIO::setID(byte id) {
  CV::set(EE_CANID, id);
  m_CANID = id;
  trace("set CANID %u(ee %u)", m_CANID, CV::get(EE_CANID) );
  m_Prot->setID(m_CANID);
}

//---------- Init all board pins.
void GPIO::init() {
  m_IO->initIO(MAX_PORTS);
}


//---------- Dump the 16 port states.
void GPIO::dumpPortState() {
  trace("%X %X %X %X %X %X %X %X : %X %X %X %X %X %X %X %X", 
  m_IO->getPortState( 0), m_IO->getPortState( 1), m_IO->getPortState( 2), m_IO->getPortState( 3), 
  m_IO->getPortState( 4), m_IO->getPortState( 5), m_IO->getPortState( 6), m_IO->getPortState( 7),
  m_IO->getPortState( 8), m_IO->getPortState( 9), m_IO->getPortState(10), m_IO->getPortState(11), 
  m_IO->getPortState(12), m_IO->getPortState(13), m_IO->getPortState(14), m_IO->getPortState(15));
}


bool GPIO::doServo(iCANFrame frame, iCmdData cmd, byte val) {
  if( m_Servos != NULL ) {
    byte pos   = cmd->data[0];
    byte tick  = cmd->data[1];
    byte param = cmd->data[2];
    return m_Servos->doServo(frame, cmd->addr, pos, tick, val, param);
  }
  return false;
}

bool GPIO::tick10ms(iCANFrame frame) {
  bool response = false;
  if( m_Servos != NULL ) {
    CmdData cmddata;
    cmddata.cmd = 0;
    if( m_Servos->tick10ms(frame, &cmddata) ) {
      response = m_Prot->makeFrame(frame, &cmddata);
    }
  } 
  m_IO->tick10ms();
  return response; 
}

//---------- Process a CAN frame and scan inputs.
bool GPIO::Process(iCANFrame frame) {
  bool response  = false;

  if( millis() - m_LastMillis >= 100 ) {
    m_LastMillis = millis();
    m_IO->increasePortTimers();
  }

  CmdData cmddata;
  memset( &cmddata, 0, sizeof(CmdData) );
  bool eval = m_Prot->evalFrame(frame, &cmddata);

  if( eval && cmddata.cmd > 0 ) {
    m_IO->setLED(true);
    trace("CBusIO processing datacnt=%d", cmddata.datacnt);

    if( cmddata.thisid ) {
      
       // Request read of node variable
      if( cmddata.cmd == CMD_GETCV ) {
        trace("get cv %u", cmddata.addr);
        cmddata.cmd = 0;

        if( cmddata.addr == 0 ) {
          cmddata.cmd   = CMD_GETCV;
          cmddata.value = CV::get(EE_SYNCID);
          trace("get node SYNCID=%X", frame->data[4]);
        }
        else if( cmddata.addr == 1 ) {
          cmddata.cmd   = CMD_GETCV;
          cmddata.value = CV::get(EE_CONFIG);
          trace("get node config=%X", cmddata.value);
        }
        else if( cmddata.addr > 1 && cmddata.addr < 18 ) {
          cmddata.cmd   = CMD_GETCV;
          cmddata.value = CV::get(EE_PORTCFG + (cmddata.addr-2));
          trace("get port %u config=%X", (cmddata.addr-2), cmddata.value);
        }
        else if( cmddata.addr == 18 || cmddata.addr == 19 ) {
          savePortState();
          cmddata.cmd   = CMD_GETCV;
          if( cmddata.addr == 18 )
            cmddata.value = CV::get(EE_PORTSTAT1);
          else
            cmddata.value = CV::get(EE_PORTSTAT2);
          trace("port state %X", cmddata.value);
        }
        else if( cmddata.addr == 20 ) {
          cmddata.cmd   = CMD_GETCV;
          cmddata.value = CV::get(EE_CANID);
          trace("get CANID %u", cmddata.value );
        }
        response = m_Prot->makeFrame(frame, &cmddata);
      }

      // Set a node variable
      else if( cmddata.cmd == CMD_SETCV ) {
        if( cmddata.addr == 0 ) {
          CV::set(EE_SYNCID, cmddata.value);
          trace("set SYNCID=%u", cmddata.value);
        }
        else if( cmddata.addr == 1 ) {
          CV::set(EE_CONFIG, cmddata.value);
          trace("set config=%02X", cmddata.value);
        }
        else if( cmddata.addr > 1 && cmddata.addr < 18 ) {
          CV::set(EE_PORTCFG + (cmddata.addr-2), cmddata.value);
          m_IO->initIO((cmddata.addr-2));
          trace("set port %u config=%02X", (cmddata.addr-2), cmddata.value);
        }
        else if( cmddata.addr == 20 ) {
          setID( cmddata.value );
        }
        else {
          trace("set cv %u=%u ???", cmddata.addr, cmddata.value);
        }
        cmddata.cmd = CMD_SETCV;
        response = m_Prot->makeFrame(frame, &cmddata);
      }

      // Signal aspect command
      else if( cmddata.cmd == CMD_SIGNAL ) {
        response = processSignal(frame, &cmddata);
      }

    }

    // General OPCs
    if( cmddata.cmd == EVT_POWER_OFF ) {
      trace("power off, save output state");
      savePortState();
    }

    if( cmddata.cmd == EVT_POWER_ON ) {
      trace("power on");
    }

    if( cmddata.cmd == CMD_QUERY ) {
      trace("publish...");
      cmddata.cmd = CMD_PUBLISH;
      response = m_Prot->makeFrame(frame, &cmddata);
    }
  
    if( cmddata.cmd == CMD_ID ) {
      trace("CAN id=%", m_CANID);
      cmddata.cmd = CMD_ID;
      cmddata.id  = m_CANID;
      response = m_Prot->makeFrame(frame, &cmddata);
    }
  
    if( cmddata.cmd == CMD_STATUS ) {
      trace("status...");
      cmddata.cmd = CMD_STATUS;
      cmddata.id  = m_CANID;
      response = m_Prot->makeFrame(frame, &cmddata);
    }
  
    if( cmddata.cmd == CMD_SOD ) {
      trace("SoD...");
      SoD();
    }
  
    if( cmddata.cmd == CMD_QUERY_SENSOR ) {
      if( cmddata.id == m_CANID && cmddata.addr < 16 ) {
        cmddata.cmd   = EVT_SENSOR;
        cmddata.addr  = cmddata.addr;
        cmddata.value = m_IO->getPortState(cmddata.addr);
        response = m_Prot->makeFrame(frame, &cmddata);
      }
    }
  
    if( cmddata.cmd == ACK_SENSOR ) {
      m_Ack = true;
      if( cmddata.id == m_CANID && cmddata.addr < 16 ) {
        //trace("ack %d,%d", cmddata.addr, cmddata.value);
        m_IO->ackInput(cmddata.addr, cmddata.value);
      }
    }
  
    // Output OPCs
    if( cmddata.cmd == CMD_OUTPUT_ON ) {
      if( cmddata.datacnt > 0 ) {
        // Servo
        response = doServo(frame, &cmddata, 1);
      }
      else {
        // Output on
        response = setOutput(frame, &cmddata, true);
      }
    }
    if( cmddata.cmd == CMD_OUTPUT_OFF ) {
      if( cmddata.datacnt > 0 ) {
        // Servo
        response = doServo(frame, &cmddata, 0);
      }
      else {
        // Output off
        response = setOutput(frame, &cmddata, false);
      }
    }

    // Input OPCs
    if( cmddata.cmd == EVT_SENSOR ) {
      // Synchronise output on
      if( !cmddata.thisid )
        response = setOutput(frame, &cmddata, cmddata.value?true:false);
    }

    //m_IO->setLED(false);
  }

  if( !response && m_Publish ) {
    m_Publish = false;
    cmddata.cmd = CMD_PUBLISH;
    response = m_Prot->makeFrame(frame, &cmddata);
  }

  if( !response ) {
    response = scanInputs(frame, &cmddata);
  }
  
  if( !response ) {
    response = resetOutputs(frame, &cmddata);
  }

  if( response )
    m_IO->setLED(true);

  return response;
}


//---------- Process aspect signal command.
/*
 * addr   = base address
 * value  = number of aspects
 * ivalue = aspect value
 */
bool GPIO::processSignal(iCANFrame frame, iCmdData cmd) {
  for( byte i = 0; i < cmd->value; i++ ) {
    byte blink = 0;
    if( (cmd->ivalue & (0x01 << i)) && (cmd->ivalue & (0x10 << i)) ) {
      blink = true;
    }
    m_IO->setOutput(cmd->addr-1+i, cmd->ivalue & (1 << i), false); 
    m_IO->setPortState(cmd->addr-1+i, cmd->ivalue & (1 << i), blink);
    m_IO->setPortDuration(cmd->addr-1+i, 5);
  }
  return m_Prot->makeFrame(frame, cmd);
}


//---------- Save the output port state in EEPROM after a power off command.
void GPIO::savePortState() {
  byte state1 = 0;
  byte state2 = 0;
  for( byte i = 0; i < 8; i++ ) {
    state1 |= m_IO->getPortState(i) << i;
    state2 |= m_IO->getPortState(i+8) << i;
  }
  CV::set(EE_PORTSTAT1, state1);
  CV::set(EE_PORTSTAT2, state2);
}


//---------- Restore the saved port state from EEPROM.
void GPIO::restorePortState() {
  if( CV::get(EE_CONFIG) & CFG_SAVEOUTPUT ) {
    byte state1 = CV::get(EE_PORTSTAT1);
    byte state2 = CV::get(EE_PORTSTAT2);
    for( byte i = 0; i < 8; i++ ) {
      if( state1 & (1 << i) && (CV::get(EE_PORTCFG+i) & PORTCFG_IO) == PORTCFG_OUT ) {
        if( !(CV::get(EE_PORTCFG+i) & PORTCFG_PULSE) ) {
          m_IO->setOutput(i, true, (CV::get(EE_PORTCFG+i)&PORTCFG_INV) ? true:false); 
          m_IO->setPortState(i, true);
        }
      }

      if( state2 & (1 << i) && (CV::get(EE_PORTCFG+i+8) & PORTCFG_IO) == PORTCFG_OUT ) {
        if( !(CV::get(EE_PORTCFG+i+8) & PORTCFG_PULSE) ) {
          m_IO->setOutput(i+8, true, (CV::get(EE_PORTCFG+i+8)&PORTCFG_INV) ? true:false); 
          m_IO->setPortState(i+8, true);
        }
      }
            
    }
  }
}


//---------- Check all pulse output port timers for setting the port to off.
bool GPIO::resetOutputs(iCANFrame frame, iCmdData cmd) {
  for( byte i = 0; i < MAX_PORTS; i++ ) {
    byte conf = CV::get(EE_PORTCFG+i);
    
    if( m_IO->isPortBlink(i) && (conf & PORTCFG_IO) == PORTCFG_OUT ) {
      if(m_IO->isPortTimer(i) ) {
        m_IO->setOutput(i, !m_IO->getPortState(i), false); 
        m_IO->setPortState(i, !m_IO->getPortState(i), true);
        m_IO->setPortDuration(i, 5);
        return false;
      }      
    }
    else if( conf & PORTCFG_PULSE && (conf & PORTCFG_IO) == PORTCFG_OUT ) {
      if(m_IO->isPortTimer(i) ) {
        trace("pulse off %d", i);
        m_IO->resetPortTimer(i);
        cmd->thisid = true; 
        cmd->addr   = i + 1;
        return setOutput(frame, cmd, false);
      }
    }
  }
  return false;
}


//---------- Scan all inputs for changes, the SoD flag will force reporting.
bool GPIO::scanInputs(iCANFrame frame, iCmdData cmd) {
  bool response = false;

  if( m_SoD ) {
    m_SoD = false;
    m_IO->SoD();
  }

  byte conf = CV::get(EE_PORTCFG+m_ScanIdx);
  
  if( (conf & PORTCFG_IO) == PORTCFG_IN ) { 
    bool report = true;
    bool state = m_IO->getInput(m_ScanIdx);

    if( conf & PORTCFG_INV ) {
      state = !state;
    }

    if( m_IO->getPortState(m_ScanIdx) != SOD && conf & PORTCFG_OFFDELAY ) {
      if( state ) {
        m_IO->setPortDuration(m_ScanIdx, 20);
      }
      else {
        if(m_IO->isPortTimer(m_ScanIdx) ) {
          // report off
          trace("report off delay");
          m_IO->resetPortTimer(m_ScanIdx);
        }
        else {
          state  = true;
          report = false;
        }
      }
    }

    bool ack = true;
    if( m_Ack && state && (state == m_IO->getPortState(m_ScanIdx)) && !m_IO->isAckInput(m_ScanIdx) ) {
      //trace("noack %d state=%d", m_ScanIdx, state);
      ack = false;
    }
        
    if( !ack || (report && state != m_IO->getPortState(m_ScanIdx)) ) {
      m_IO->setPortState(m_ScanIdx, state, false, state?true:false);
        
      cmd->cmd    = EVT_SENSOR;
      cmd->addr   = m_ScanIdx;
      cmd->value  = state;
      cmd->ivalue = m_IO->getPortValue(m_ScanIdx);
      response = m_Prot->makeFrame(frame, cmd);
    }
  }

  m_ScanIdx++;
  if( m_ScanIdx >= MAX_PORTS )
    m_ScanIdx = 0;

  return response;
}


//---------- Set an output port regarding pulse timers.
bool GPIO::setOutput(iCANFrame frame, iCmdData cmd, bool on) {
  unsigned int port = cmd->addr;

  if( port > 0 && port <= MAX_PORTS ) {
    byte conf = CV::get(EE_PORTCFG+(port-1));
    
    if( (conf & PORTCFG_IO) == PORTCFG_OUT ) {
      if( cmd->thisid || (CV::get(EE_SYNCID) == cmd->id && (conf & PORTCFG_SYNC) ) ) {
        if( on && conf & PORTCFG_PULSE ) {
          byte pulsetime = CV::get(EE_CONFIG) & CFG_PULSETIME_MASK;
          if( pulsetime & CFG_PULSETIME_500 )
            m_IO->setPortDuration(port-1, 5);
          else if( pulsetime & CFG_PULSETIME_1000 )
            m_IO->setPortDuration(port-1, 10);
          else if( pulsetime & CFG_PULSETIME_2000 )
            m_IO->setPortDuration(port-1, 20);
          else
            m_IO->setPortDuration(port-1, 3);
        }
        trace("set port 0x%X-%u to %s", cmd->id, port, on ? "ON":"OFF");
        m_IO->setOutput(port-1, on, (conf&PORTCFG_INV) ? true:false); 
        m_IO->setPortState(port-1, on);
  
        cmd->cmd   = EVT_OUTPUT;
        cmd->addr  = port;
        cmd->value = on;
        return m_Prot->makeFrame(frame, cmd);
      }
    }
  }

  return false;
}
