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

#include <stdlib.h>
#include <time.h>

//---------- MBus constructor.
MBus::MBus(byte ID, byte BoardType) {
  randomSeed(time(NULL));
  m_Hash = 0;
  m_CANID = ID;
  m_UID = m_CANID + GCA_BASE_UID;
  trace("MBUS ID=0x%04X", m_UID );
  m_BoardType = BoardType;
}

int MBus::getCANBPS() {
  return 250; 
}

byte MBus::getFilters(unsigned long* filters) {
  return 0;
}

void MBus::setID(byte id) {
  m_CANID = id;
  m_UID   = m_CANID + GCA_BASE_UID;
}

bool MBus::evalFrame(iCANFrame frame, iCmdData cmd) {
  bool eval = false;
  cmd->cmd = 0;

  if( frame->id > 0 ) {
    unsigned int l_UID = 0;
    bool rsp = false;
    switch( getCmd(frame, &rsp) ) {
      case SENSOR_EVENT:
        if( !rsp ) {
          cmd->cmd  = CMD_QUERY_SENSOR;
          cmd->id   = frame->data[0] * 256 + frame->data[1];
          cmd->addr = frame->data[2] * 256 + frame->data[3];
          eval = true;
        }
        else if(rsp && frame->dlc == 6) {
          // Sensor acknowledge
          cmd->cmd   = ACK_SENSOR;
          cmd->id    = frame->data[0] * 256 + frame->data[1];
          cmd->addr  = frame->data[2] * 256 + frame->data[3];
          cmd->value = frame->data[5];
          eval = true;
        }
      break;
      case SENSOR_POLL:
        if( !rsp ) {
          cmd->cmd = CMD_SOD;
          eval = true;
        }
      break;
      case CMD_STATUS_DATA:
        if( !rsp ) {
          if( frame->data[0] == 0 && frame->data[1] == 0 && frame->data[2] == (m_UID >> 8) && frame->data[3] == (m_UID & 0xFF) ) {
            cmd->cmd = CMD_STATUS;
            cmd->thisid = true;
            cmd->value = frame->data[4]; // Channel index.
            eval = true;
          }
        }
      break;
      case CMD_SYSTEM:
        if( !rsp && frame->data[4] == CMD_SYSSUB_ID ) {
          if( frame->data[0] == 0 && frame->data[1] == 0 && frame->data[2] == (m_UID >> 8) && frame->data[3] == (m_UID & 0xFF) ) {
            cmd->cmd = CMD_ID;
            cmd->thisid = true;
            eval = true;
          }
        }
      break;
      case CMD_PING:
        if( !rsp ) {
          cmd->cmd = CMD_QUERY;
          eval = true;
        }
      break;
      case CMD_ACC_CONFIG:
        if( frame->dlc == 7 ) {
          l_UID = frame->data[2] * 256 + frame->data[3];
          cmd->cmd = CMD_SETCV;
          cmd->addr  = frame->data[4] * 256 + frame->data[5];
          cmd->value = frame->data[6];
          cmd->thisid = (l_UID == m_UID) ? true:false;
          eval = true;
        }
        else if( frame->dlc == 6 ) {
          l_UID = frame->data[2] * 256 + frame->data[3];
          cmd->cmd = CMD_GETCV;
          cmd->addr  = frame->data[4] * 256 + frame->data[5];
          cmd->value = frame->data[6];
          cmd->thisid = (l_UID == m_UID) ? true:false;
          eval = true;
        }
        break;
      case CMD_ACC_SWITCH:
        l_UID = frame->data[2] * 256 + frame->data[3];
        if( l_UID - m_UID > 0 && l_UID - m_UID <= MAX_PORTS ) {
          // Short event
          cmd->cmd  = (frame->data[4] > 0 ? CMD_OUTPUT_ON:CMD_OUTPUT_OFF);
          cmd->id   = l_UID;
          cmd->addr = l_UID - m_UID;
          cmd->thisid = true;
          eval = true;
        }
        else if( l_UID == m_UID ) {
          if( frame->data[6] > 0 ) {
            // signal aspect command
            cmd->cmd    = CMD_SIGNAL;
            cmd->value  = frame->data[6]; // number of aspects
            cmd->ivalue = frame->data[4] * 256 + frame->data[5]; // aspect value
          }
          else {
            // Long event
            cmd->cmd  = (frame->data[4] > 0 ? CMD_OUTPUT_ON:CMD_OUTPUT_OFF);
          }
          cmd->id   = l_UID;
          cmd->addr = frame->data[7];
          cmd->thisid = true;
          eval = true;
        }
        break;
    }
  }
  return eval;
}

bool MBus::makeFrame(iCANFrame frame, iCmdData cmd) {
  bool didmake = false;
  
  if( cmd->cmd == CMD_GETCV ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    frame->data[2] = m_UID >> 8;
    frame->data[3] = m_UID & 0xFF;
    frame->data[4] = cmd->addr / 256;
    frame->data[5] = cmd->addr % 256;
    frame->data[6] = cmd->value;
    makeFrame(frame, PRIO_ACCESSORY, CMD_ACC_CONFIG, 7, frame->data, true);
    didmake = true;
  }
  else if( cmd->cmd == CMD_PUBLISH ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    frame->data[2] = (m_UID >> 8);
    frame->data[3] = (m_UID & 0xFF);
    frame->data[4] = RCAN_VERMAJ;
    frame->data[5] = RCAN_VERMIN;
    frame->data[6] = MANU_ROCRAIL;
    frame->data[7] = m_BoardType;
    makeFrame(frame, PRIO_PING, CMD_PING, 8, frame->data, true);
    didmake = true;
  }
  else if( cmd->cmd == CMD_ID ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    frame->data[2] = (m_UID >> 8);
    frame->data[3] = (m_UID & 0xFF);
    frame->data[4] = CMD_SYSSUB_ID;
    frame->data[5] = 0;
    frame->data[6] = cmd->id;
    makeFrame(frame, PRIO_PING, CMD_SYSTEM, 7, frame->data, true);
    didmake = true;
  }
  else if( cmd->cmd == CMD_STATUS ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    frame->data[2] = (cmd->id >>8);
    frame->data[3] = (cmd->id & 0xFF);
    frame->data[4] = cmd->value;
    frame->data[5] = 0;
    makeFrame(frame, PRIO_PING, CMD_STATUS_DATA, 6, frame->data, true);
    didmake = true;
  }
  else if( cmd->cmd == EVT_OUTPUT ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    if( CV::get(EE_CONFIG) & CFG_SHORTEVENTS ) {
      frame->data[2] = (m_UID + cmd->addr) >> 8;
      frame->data[3] = (m_UID + cmd->addr) & 0xFF;
      frame->data[4] = cmd->value;
      frame->data[5] = 1;
      makeFrame(frame, PRIO_ACCESSORY, CMD_ACC_SWITCH, 6, frame->data, true);
    }
    else {
      frame->data[2] = m_UID  >> 8;
      frame->data[3] = m_UID & 0xFF;
      frame->data[4] = cmd->value;
      frame->data[5] = 1;
      frame->data[6] = cmd->addr >> 8;
      frame->data[7] = cmd->addr & 0xFF;
      makeFrame(frame, PRIO_ACCESSORY, CMD_ACC_SWITCH, 8, frame->data, true);
    }
    didmake = true;
  }
  else if( cmd->cmd == CMD_SIGNAL ) {
    frame->data[0] = 0;
    frame->data[1] = 0;
    frame->data[2] = cmd->id >> 8;
    frame->data[3] = cmd->id & 0xFF;
    frame->data[4] = cmd->ivalue >> 8;
    frame->data[5] = cmd->ivalue & 0xFF;
    frame->data[6] = cmd->value;
    frame->data[7] = cmd->addr;
    makeFrame(frame, PRIO_SENSORS, CMD_ACC_SWITCH, 8, frame->data, true);
    didmake = true;
  }
  else if( cmd->cmd == EVT_SENSOR ) {
    frame->data[0] = m_CANID >> 8;
    frame->data[1] = m_CANID & 0xFF;
    frame->data[2] = cmd->addr >> 8;
    frame->data[3] = cmd->addr & 0xFF;
    frame->data[4] = (cmd->value > 0 ? 0:1);
    frame->data[5] = cmd->value;
    frame->data[6] = cmd->ivalue >> 8;
    frame->data[7] = cmd->ivalue & 0xFF;
    makeFrame(frame, PRIO_SENSORS, SENSOR_EVENT, 8, frame->data, true);
    didmake = true;
  }
  return didmake;
}


unsigned int MBus::getCmd(iCANFrame frame, bool* rsp) {
  unsigned int cmd = (frame->id & 0x01FF0000) >> 17;
  *rsp = (frame->id & 0x00010000) ? true:false;
  return cmd; // remove response flag
}


void MBus::makeFrame(iCANFrame frame, byte prio, unsigned int cmd, byte len, byte* data, bool rsp ) {
  if( m_Hash == 0 )
    genHash();
    
  cmd |= (prio << 8);
  cmd <<= 1;
  cmd |= (rsp?MBUS_RESPONSE:0);
  frame->id = cmd;
  frame->id <<= 16;
  frame->id += m_Hash;
  frame->ext = 1;
  frame->dlc = len;
  for( byte i = 0; i < len; i++ )
    frame->data[i] = data[i];
}


void MBus::genHash() {
  unsigned int l_hash = random(8192);
  unsigned idh = (l_hash >> 8) & 0x1F;
  unsigned idl = l_hash & 0xFF;
  m_Hash  = idl & 0x7F;
  m_Hash |= (idl & 0x80) << 3;
  m_Hash |= idh << 11;
  m_Hash |= 0x0300;
  trace("hash 0x%04X", m_Hash );
}
