/*
 Rocrail - Model Railroad Software
 Copyright (c) 2002-2022 Robert Jan Versluis, Rocrail.net
 All rights reserved.
*/
#include <mcp_can.h>
#include <EEPROM.h>
#include <WiFi.h>
#include "RCAN_dfs.h"
#include "board.h"
#include "gpio.h"
#include "trace.h"
#include "cv.h"


static char ms_HostName[16] = {'\0'};
struct WiFiCnf wificnf;
WiFiServer* server = NULL;
WiFiClient client[1];

#define PORT 5550

#define MONITOR_BPS 500000

// The CAN object:
#ifdef ARDUINO_ARCH_RP2040
static MCP_CAN CAN0(PIN_SPI0_SS); // Set CS to pin 1 [SPI0 CSn]
#else
static MCP_CAN CAN0(10); // Set CS to pin 10
#endif

// The target board:
Board* ms_Board = null;

static bool ms_USB    = false;
static bool ms_CANOK  = false;
static int  ms_MCPrc  = CAN_OK;

//---------- Setup the Arduino.
void setup() {
  // Init terminal
  Serial.begin(MONITOR_BPS);
  while(!Serial); // for debugging only

  CV::init();
  if( CV::get(0) == 0xFF ) {
    trace("reset CV");
    CV::resetAll();
  }
  
  setDebug( (CV::get(EE_CONFIG) & CFG_DEBUG) ? true:false );
  trace("CV ready");
  CV::readWiFiCnf(&wificnf);

  INT8U bps = CAN_125KBPS;
  INT8U mhz = MCP_16MHZ;

  byte l_BoardConfig = CV::get(EE_BOARD);
  ms_USB = (l_BoardConfig & USB_INTERFACE);
  
  if( (l_BoardConfig & CLOCK_MASK) == CLOCK_8MHZ )
    mhz = MCP_8MHZ;
  
  // Select the wanted target board:
  byte prot = PROTOCOL_CBUS;
  if( (l_BoardConfig & PROTOCOL_MASK) == PROTOCOL_MBUS )
    prot = PROTOCOL_MBUS;
    
  ms_Board = new GPIO(prot, (l_BoardConfig & BOARD_MASK) );
  
  if( ms_Board->getCANBPS() == 250 )
    bps = CAN_250KBPS;
  else
    bps = CAN_125KBPS;

  trace("CAN bps=%d, MCP2515 %dMHz USB=%d cnf=%02X", ms_Board->getCANBPS(), (mhz == MCP_16MHZ ?16:8), ms_USB, l_BoardConfig);

  // Initialize MCP2515 running at 8MHz with a baudrate of bps and the masks and filters disabled.
  INT8U rc = CAN_OK;
  if( (rc = CAN0.begin(MCP_STDEXT, bps, mhz)) == CAN_OK ) {
    ms_CANOK = true;
    // Check for filters...
    if( ms_Board != null ) {
      unsigned long filters[MAX_FILTERS+1];
      byte nr = ms_Board->getFilters(filters);
      if( nr > 0 ) {
        // filters[0] is mask
        trace("mask=%08lX", filters[0]);
        rc  = CAN0.init_Mask(0, 1, filters[0]); // Init first mask...
        rc |= CAN0.init_Mask(1, 1, filters[0]); // Init first mask...
        if( rc == CAN_OK ) {
          for( byte i = 0; i < nr && i < MAX_FILTERS; i++ ) {
            trace("filters[%d]=%08lX", i, filters[i+1]);
            rc  = CAN0.init_Filt(i,1,filters[i+1]);
            rc |= CAN0.init_Filt(i+2,1,filters[i+1]);
            if( rc != CAN_OK )
              trace("init filter rc=%d", rc);
          }
        }
        else {
          trace("init mask rc=%d", rc);
        }
      }
    }
    // Set operation mode to normal so the MCP2515 sends acks to received data.
    CAN0.setMode(MCP_NORMAL);
    trace("MCP2515 ready...");
  }
  else {
    /*
    #define CAN_OK             (0)
    #define CAN_FAILINIT       (1)
    #define CAN_FAILTX         (2)
    #define CAN_MSGAVAIL       (3)
    #define CAN_NOMSG          (4)
    #define CAN_CTRLERROR      (5)
    #define CAN_GETTXBFTIMEOUT (6)
    #define CAN_SENDMSGTIMEOUT (7)
    #define CAN_FAIL       (0xff)
    */
    ms_MCPrc = rc;
    trace("Error %d MCP2515", rc);
  }

#ifdef ARDUINO_ARCH_RP2040
  if( rp2040.isPicoW() ) {
    server = new WiFiServer(PORT);
    server->begin();
    WiFi.mode(WIFI_STA);
    sprintf(ms_HostName, "RCAN%d", CV::get(EE_CANID) );
    WiFi.setHostname(ms_HostName);
    if( strlen(wificnf.ssid) > 0 && strlen(wificnf.ssid) > 0 ) {
      WiFi.begin(wificnf.ssid, wificnf.pwd);
    }
  }
#endif

}



//---------- Send a frame
static bool sendFrame(iCANFrame frame) {
  if( !ms_CANOK )
    return false;

  INT8U rc = CAN0.sendMsgBuf(frame->id, frame->ext, frame->dlc, frame->data);
  if( rc == CAN_OK ) {
    //trace("CAN did send data id=%X", frame->id);
    return true;
  }
  else {
    trace("CAN send error %d", rc);
  }
  return false;
}


void sendWiFi( iCANFrame frame ) {
  if( server != NULL && client[0] ) {
    if(client[0].connected() ) {
      char sFrame[30] = {'\0'};
      makeASCIIFrame(frame, sFrame);
      client[0].println(sFrame);
    }
  }
}

//---------- The main loop which scans for CAN and console messages.
// Timer tickers:
static unsigned long m_1000mS = 0;
static unsigned long m_200mS  = 0;
static unsigned long m_100mS  = 0;
static unsigned long m_10mS   = 0;
static unsigned long m_5mS    = 0;

//---------- Loop
void loop() {
  unsigned long l_Millis = millis();
  bool ms5    = false;
  bool ms10   = false;
  bool ms100  = false;
  bool ms200  = false;
  bool ms1000 = false;

  unsigned long ms = millis();
      
  // The 1000ms ticker.
  if( l_Millis - m_1000mS >= 1000 ) {
    m_1000mS = ms;
    ms1000 = true;
  }
  // The 200ms ticker.
  if( l_Millis - m_200mS >= 200 ) {
    m_200mS = ms;
    ms200 = true;
  }
  // The 100ms ticker.
  if( l_Millis - m_100mS >= 100 ) {
    m_100mS = ms;
    ms100 = true;
  }
  // The 10ms ticker.
  if( l_Millis - m_10mS >= 10 ) {
    m_10mS = ms;
    ms10 = true;
  }
  // The 5ms ticker.
  if( l_Millis - m_5mS >= 5 ) {
    m_5mS = ms;
    ms5 = true;
  }

  CANFrame frame;

  frame.id    = 0;
  frame.dlc   = 0;
  frame.state = CAN_BUFFER_NOP;


  if( server != NULL && WiFi.status() == WL_CONNECTED ) {
    if( !client[0] || !client[0].connected() ) {
      client[0] = server->available();
      if( client[0] )
        trace("CAN Host connected %s", client[0].remoteIP().toString().c_str() );
    }
  }

  INT8U rc = ms_CANOK ? CAN0.readMsgBuf((uint32_t*)&frame.id, &frame.ext, &frame.dlc, frame.data) : CAN_FAIL;
  if( rc == CAN_OK ) {
    frame.state = CAN_BUFFER_DATA;
    trace("CAN data received");
    if( ms_USB ) 
      sendASCIIFrame(&frame);
  }
  else {
    if( checkConsole(&frame) ) {
      trace("USB frame id=%08X dlc=%u ext=%u data=%02X %02X %02X %02X %02X %02X %02X %02X", 
          frame.id, frame.dlc, frame.ext, frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5], frame.data[6], frame.data[7]);
      if( ms_USB ) 
        sendFrame(&frame);
    }
    else if( server != NULL && client[0] ) {
      if(client[0].connected() && client[0].available()) {
        String req = client[0].readStringUntil('\n');
        trace("WiFi %s", req.c_str());
        ASCIIFrame2CANFrame((char*)req.c_str(), &frame);
      }
    }
  }


  if( ms_Board != null ) {
    if( ms_Board->Process(&frame) ) {
      if( sendFrame(&frame) ) {
      }
      sendWiFi(&frame);  
      if( ms_USB ) 
        sendASCIIFrame(&frame);
    }
  }

  if( ms10 ) {
    if( ms_Board != null ) {
      if( ms_Board->tick10ms(&frame) ) {
        if( sendFrame(&frame) ) {
        }
        sendWiFi(&frame);  
        if( ms_USB ) 
          sendASCIIFrame(&frame);
      }
    }
  }
  
}


//---------- Convert two ASCII chars to a binary byte value.
byte HEXA2Byte( const byte* s ) {
  byte val = 0;
  if( s[0] >= 'A' && s[0] <= 'F' ) val += (s[0] - 'A') + 10;
  else if( s[0] >= 'a' && s[0] <= 'f' ) val += (s[0] - 'a') + 10;
  else val += (s[0] - '0');
  val <<= 4;
  if( s[1] >= 'A' && s[1] <= 'F' ) val += (s[1] - 'A') + 10;
  else if( s[1] >= 'a' && s[1] <= 'f' ) val += (s[1] - 'a') + 10;
  else val += (s[1] - '0');
  return val;
}


//---------- Convert ASCII frame
void ASCIIFrame2CANFrame(char* consoleCmd, iCANFrame frame) {
  frame->id  = 0;
  frame->dlc = 0;
  frame->ext = 0;
  memset( frame->data, 0, 8 );

  // Standard frame :SC020N71001801;
  if( consoleCmd[1] == 'S' ) {
    byte idh = HEXA2Byte((const byte*)consoleCmd + 2);
    byte idl = HEXA2Byte((const byte*)consoleCmd + 4);
    frame->id = (idl >> 5 ) + ((idh&0x1F) << 3);
    frame->ext = 0;
    if( consoleCmd[6] == 'N' ) {
      for( byte i = 0; i < 8; i++ ) {
        if( consoleCmd[7+i*2] == ';' || consoleCmd[7+i*2] == '\0' || consoleCmd[7+i*2] == '\n' || consoleCmd[7+i*2] == '\r' )
          break; 
        frame->data[i] = HEXA2Byte((const byte*)consoleCmd + 7+i*2);
        frame->dlc++;
      }
    }
  }
  
  // Extended frame
  else if(consoleCmd[1] == 'X') {
    unsigned long idh = HEXA2Byte((const byte*)consoleCmd + 2);
    unsigned long idl = HEXA2Byte((const byte*)consoleCmd + 4);
    unsigned long edh = HEXA2Byte((const byte*)consoleCmd + 6);
    unsigned long edl = HEXA2Byte((const byte*)consoleCmd + 8);
    frame->id += idh << 24;
    frame->id += idl << 16;
    frame->id += edh << 8;
    frame->id += edl;
    frame->ext = 1;
    if( consoleCmd[10] == 'N' ) {
      for( byte i = 0; i < 8; i++ ) {
        if( consoleCmd[11+i*2] == ';' || consoleCmd[11+i*2] == '\0' || consoleCmd[11+i*2] == '\n' || consoleCmd[11+i*2] == '\r' )
          break; 
        frame->data[i] = HEXA2Byte((const byte*)consoleCmd + 11+i*2);
        frame->dlc++;
      }
    }
  }
  else {
    trace("unsupported [%s]", consoleCmd);
  }
}


//---------- Check the serial monitor for commands
bool checkConsole(iCANFrame frame) {
static char consoleCmd[32];
static byte consoleIdx = 0;
  if( Serial.available() && consoleIdx < 32 ) {
    consoleCmd[consoleIdx] = (char)Serial.read();
    if( consoleCmd[consoleIdx] == '\n' || consoleCmd[consoleIdx] == '\r' || consoleCmd[consoleIdx] == '\0' ) {
      consoleCmd[consoleIdx] = '\0';
      consoleIdx = 0;

      if( ms_USB && consoleCmd[0] == ':' ) {
        ASCIIFrame2CANFrame(consoleCmd, frame);
        return true;  
      }
      else if( consoleCmd[0] == '?' ) {
        setDebug(true);
        const char* boardType = "";
      #if defined(__AVR_ATmega4809__)
        boardType = " Nano Every"; // Nano Every
      #elif defined(__AVR_ATmega328P__)
        boardType = " Nano Classic";      
      #elif defined(__AVR_ATmega328PB__)
        boardType = " ATMega328PB";      
      #endif        

      #ifdef ARDUINO_ARCH_RP2040
        if( rp2040.isPicoW() )
          boardType = " PicoW";
        else
          boardType = " Pico";
      #endif

        /*
        typedef enum {
            WL_NO_SHIELD = 255,
            WL_NO_MODULE = WL_NO_SHIELD,
            WL_IDLE_STATUS = 0,
            WL_NO_SSID_AVAIL,
            WL_SCAN_COMPLETED,
            WL_CONNECTED,
            WL_CONNECT_FAILED,
            WL_CONNECTION_LOST,
            WL_DISCONNECTED,
            WL_AP_LISTENING,
            WL_AP_CONNECTED,
            WL_AP_FAILED
        } wl_status_t;
        */
        byte l_BoardConfig = CV::get(EE_BOARD);
        trace("RCAN:%d%s %d.%d.%d", CV::get(EE_CANID), boardType, RCAN_VERMAJ, RCAN_VERMIN, getRevision() );
        trace("CAN bps=%d, MCP2515 %dMHz USB=%d cnf=%02X MCPrc=%d SSID=%s pass=%s wifi=%d rssi=%d", ms_Board->getCANBPS(), 
          ((l_BoardConfig & CLOCK_MASK) == CLOCK_8MHZ ?8:16), ms_USB, l_BoardConfig, ms_MCPrc, wificnf.ssid, wificnf.pwd, WiFi.status(), WiFi.RSSI());
        trace("(c)2024 Robert Jan Versluis (rocrail.net)");
      }
      else if( consoleCmd[0] == '=' ) {
        if( ms_Board != null )
          ms_Board->setID( atoi(consoleCmd+1) );
      }
      else if( consoleCmd[0] == '$' ) {
        if( ms_Board != null ) {
          for( int i = 0; i < 16 && consoleCmd[1+i] != '\0'; i++ ) {
            ms_Board->setPort( i, (byte)(consoleCmd[1+i]-'0') );
          }
        }        
      }
      else if( consoleCmd[0] == '#' ) {
        if( ms_Board != null ) {
          byte port = consoleCmd[1] - '0';
          if( consoleCmd[1] > 'a' )
            consoleCmd[1] - 'a' + 10;
          ms_Board->setPort( port, (byte)(consoleCmd[2]-'0') );
        }
      }
      else if( strcmp( "dump", consoleCmd ) == 0 ) {
        dumpEEPROM(8);
      }
      else if( strcmp( "sod", consoleCmd ) == 0 ) {
        trace("SoD");
        if( ms_Board != null )
          ms_Board->SoD();
      }
      else if( strcmp( "stat", consoleCmd ) == 0 ) {
        if( ms_Board != null )
          ms_Board->dumpPortState();
      }
      else if( strcmp( "init", consoleCmd ) == 0 ) {
        if( ms_Board != null )
          ms_Board->init();
      }
      else if( strcmp( "pub", consoleCmd ) == 0 ) {
        if( ms_Board != null )
          ms_Board->publish();
      }
      else if( strcmp( "loop", consoleCmd ) == 0 ) {
        trace("Mode: Loopback");
        CAN0.setMode(MCP_LOOPBACK);
      }
      else if( strcmp( "norm", consoleCmd ) == 0 ) {
        trace("Mode: Normal");
        CAN0.setMode(MCP_NORMAL);
      }
      else if( strcmp( "mbus", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~PROTOCOL_MASK;
        l_BoardConfig |= PROTOCOL_MBUS;
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "cbus", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~PROTOCOL_MASK;
        l_BoardConfig |= PROTOCOL_CBUS;
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "wioc", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~PROTOCOL_MASK;
        l_BoardConfig |= PROTOCOL_WIOC;
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "usbon", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~USB_INTERFACE;
        l_BoardConfig |= USB_INTERFACE;
        trace("USB: on %02X", l_BoardConfig);
        CV::set(EE_BOARD, l_BoardConfig);
        ms_USB = true;
      }
      else if( strcmp( "usboff", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~USB_INTERFACE;
        trace("USB: off %02X", l_BoardConfig);
        CV::set(EE_BOARD, l_BoardConfig);
        ms_USB = false;
      }
      else if( strcmp( "16m", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~CLOCK_MASK;
        l_BoardConfig |= CLOCK_16MHZ;
        trace("16MHz %02X", l_BoardConfig);
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "8m", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~CLOCK_MASK;
        l_BoardConfig |= CLOCK_8MHZ;
        trace("8MHz %02X", l_BoardConfig);
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "gpio", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~BOARD_MASK;
        l_BoardConfig |= BOARD_OPT_NONE;
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "servo", consoleCmd ) == 0 ) {
        byte l_BoardConfig = CV::get(EE_BOARD);
        l_BoardConfig &= ~BOARD_OPT_SERVO;
        l_BoardConfig |= BOARD_OPT_SERVO;
        CV::set(EE_BOARD, l_BoardConfig);
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "resetall", consoleCmd ) == 0 ) {
        CV::resetAll();
        delay(10);
        ResetArduino();
      }
      else if( strcmp( "debugon", consoleCmd ) == 0 ) {
        byte cnf = CV::get(EE_CONFIG);
        cnf &= ~CFG_DEBUG;
        cnf |= CFG_DEBUG;
        CV::set(EE_CONFIG, cnf);
        setDebug(true);
      }
      else if( strcmp( "debugoff", consoleCmd ) == 0 ) {
        byte cnf = CV::get(EE_CONFIG);
        cnf &= ~CFG_DEBUG;
        CV::set(EE_CONFIG, cnf);
        setDebug(false);
      }
      else if( strncmp( consoleCmd, "ssid=", 5 ) == 0 ) {
        strcpy( wificnf.ssid, consoleCmd+5 );
        CV::writeWiFiCnf(&wificnf);
      }
      else if( strncmp( consoleCmd, "pass=", 5 ) == 0 ) {
        strcpy( wificnf.pwd, consoleCmd+5 );
        CV::writeWiFiCnf(&wificnf);
      }
    }
    else {
      consoleIdx++;
      consoleCmd[consoleIdx] = '\0';
    }
  }
  return false;
}

void ResetArduino() {
#ifdef ARDUINO_ARCH_RP2040
  rp2040.reboot();
#else
  asm volatile ("  jmp 0");  
#endif
}
