/*
   EyeRIS UDP Server

   A simple app to control the PRODINo Ethernet V2 board and communiate
   with the surface vessel over ethernet

   Main features:

    Control over 4 relays
    Echo of received RS485 data
    Transmision of control messages to RS485 devices and PRODINo board
    System status information from sensors external to the board

   The app is derived from UdpRelay.ino from : https://kmpelectronics.eu/tutorials-examples/prodino-ethernet-examples/

   Author: pldr
   Created: 05/3/2019
*/

#include "KmpDinoEthernet.h"
#include "KMPCommon.h"
#include <elapsedMillis.h>

#include "InternalSensors.h"

// Setup for software I2C on PRODino board using digital pins 6 and 13
/*#define SCL_PIN 6
#define SCL_PORT PORTD
#define SDA_PIN 6
#define SDA_PORT PORTB
#include <SoftI2CMaster.h>
#include <SoftWire.h>
*/

#define THERMAL_CUTOFF 50.0   // Celcius

// If in debug mode - print debug information in Serial. Comment in production code, this bring performance.
// This method is good for development and verification of results. But increases the amount of code and decreases productivity.
#define DEBUG

// Enter a MAC address and IP address for your controller below.
byte _mac[] = { 0x00, 0x08, 0xDC, 0xBB, 0x2E, 0xDF };

// The IP address will be dependent on your local network.
IPAddress _ip(192, 168, 1, 199);

// Local port.
uint16_t LOCAL_PORT = 1111;

// Define constants.
char PREFIX[] = "FF";
char _commandSend = '\0';

// Buffer for receiving data.
char _packetBuffer[UDP_TX_PACKET_MAX_SIZE];

// Buffer for receiving RS485 data
char _rs485Buffer[UDP_TX_PACKET_MAX_SIZE];

// general-purpose buffer for immediate use (do not rely on contents from one use to another)
#define BUFSIZE 50
char _buffer[BUFSIZE];

int rs485Index = 0;

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP _udp;

// general timer for status messages
elapsedMillis timeElapsed;
unsigned int statusInterval = 0;

/**
  \brief Setup void. Arduino executed first. Initialize DiNo board and prepare Ethernet connection.


  \return void
*/
void setup()
{

  // wait for usb serial to come alive
  delay(4000);
  
#ifdef DEBUG
  // Open serial communications and wait for port to open:
  Serial.begin(56000);
  //while (!Serial) {
  //	; // wait for serial port to connect. Needed for Leonardo only. If need debug setup() void.
  //}
#endif

  // Init Dino board. Set pins, start W5200.
  DinoInit();

  // Start RS485 with boud 19200 and 8N1.
  RS485Begin(9600);
  
  // Setup the BME 280 Sensor(s)
  bme280Setup();

  // Setup the INA260 sensors
  ina260Setup();

  // Start the Ethernet connection and Udp listener.
  Ethernet.begin(_mac, _ip);
  _udp.begin(LOCAL_PORT);

#ifdef DEBUG
  Serial.print("UDP: ");
  Serial.print(Ethernet.localIP());
//  Serial.println(Ethernet.gatewayIP());
//  Serial.println(Ethernet.subnetMask());
#endif
}

/**
  \brief Loop void. Arduino executed second.


  \return void
*/
void loop() {

  pinMode(13,OUTPUT);
  digitalWrite(13,0);

  // Check safety limits
  safetyCheck();
 
  // Listen for incoming packets. Execute commands in packet
  int packetSize = _udp.parsePacket();

  // Return any RS485 data (e.g., light status
  echoRS485();

  if (statusInterval > 200 && timeElapsed > statusInterval) {
    writeStatus();
    timeElapsed = 0;
  }

  if (!packetSize)
    return;

#ifdef DEBUG
  Serial.print("Receive packet of size ");
  Serial.println(packetSize);
#endif

  // If client connected On status led.
  OnStatusLed();

  ReadClientPacket();

  WriteClientResponse();

  // If client disconnected Off status led.
  OffStatusLed();

#ifdef DEBUG
  Serial.println("Client disconnected.\r\n----");
#endif
}

// Perform basic system checks.
// Specifically, turn off Camera and lights when the either temperature
// sensor reads above THERMAL_CUTOFF
//
void safetyCheck()
{
  // Check for thermal cutoff
  if (bme280_0.readTempC() > THERMAL_CUTOFF ||
      bme280_1.readTempC() > THERMAL_CUTOFF)
  {
    // All off
    for (int i = 0; i < RELAY_COUNT; i++)
    {
      SetRelayStatus(i, false);
    }
  }
}


char echoRS485() {

  int i = 0, start = 0, finish = 0;
  while ((i = RS485Read()) > -1)
  {
    Serial.println(i);
#if 0
    IntToChars(i, _buffer);
    Serial.println((char)i);
    Serial.println(_buffer);
#endif

    start = 1;
    if (i > '~' || (i < ' ' && i != '\r')) continue;      // Only printable chars

    _rs485Buffer[rs485Index++ % UDP_TX_PACKET_MAX_SIZE] = i;
    // Check for 485 data to send
    if (i == '\r') {
      finish = 1;
    }
  }
  
  if (start && !finish)
  {
    _rs485Buffer[rs485Index++ % UDP_TX_PACKET_MAX_SIZE] = '-';
    finish = 1;
  }

  if (finish)
  {
      Serial.println(_rs485Buffer);
      writeRS485Data();
  }

  rs485Index = start = finish = 0;
  return i;
}

/**
   \brief ReadClientPacket void. Read and parse client request.
 	  First row of client request is similar to:
      Prefix Command
           | |
           FFR0011
              ||||
              3  6 relay position. First is 1, second - 2...
      You can check communication client-server get program Smart Sniffer: http://www.nirsoft.net/utils/smsniff.html


   \return void
*/
void ReadClientPacket()
{
#ifdef DEBUG
  Serial.print("Read client packet from ");
  Serial.println(_udp.remoteIP());
//  Serial.print(", port ");
//  Serial.println(_udp.remotePort());
#endif

  _commandSend = '\0';

  int requestLen = 0;
  int readSize;
  while ((readSize = _udp.read(_packetBuffer, UDP_TX_PACKET_MAX_SIZE)) > 0)
  {
    requestLen += readSize;
#ifdef DEBUG
    Serial.println(_packetBuffer);
#endif
    // Packet is not valid. Read to end.
    //if(requestLen != 7)
    //{
    //    continue;
    //}

    // Check first 2 chars is prefix.
    // If Prefix not equals first 2 chars not checked any more.
    if (strncmp(_packetBuffer, PREFIX, 2) != 0)
    {
      // Send to rs485
      if (readSize < UDP_TX_PACKET_MAX_SIZE - 3) {
        _packetBuffer[readSize] = '\r';
        _packetBuffer[readSize+1] = '\n';
        _packetBuffer[readSize+2] = '\0';
        
        RS485Write(_packetBuffer);
      }
      return;
    }

    // Command R for relays.
    if (_packetBuffer[2] == CH_R)
    {
      _commandSend = CH_R;
      int statusPos = 3; // To check new statuses.
      char dataChar;
      // Read chars for On or Off for all relays.
      for (int i = 0; i < RELAY_COUNT; i++)
      {
        dataChar = _packetBuffer[statusPos];
        bool status;
        if (dataChar == CH_0 || dataChar == CH_1)
        {
          // if c == 1 - true. if c == 0 false.
          status = dataChar == CH_1;
          SetRelayStatus(i, status);
        }
        else
        {
          break; // Char is not (0, 1) valid.
        }

        statusPos++;
      }
    }
    else if (_packetBuffer[2] == 'S')
    {
      _commandSend = 'S';
      String interval = "";
      for (int i = 3; i < requestLen; i++) {
        interval += String(_packetBuffer[i]);
      }
#ifdef DEBUG
      Serial.println("\r\nParsed Interval: " + interval);
#endif
      statusInterval = interval.toInt();
    }
    else
      Serial.println("Command is not valid.");
  }

#ifdef DEBUG
  if (_commandSend == CH_R)
    Serial.println("Relay command sent.");
#endif
}

void writeStatus() {
  _udp.beginPacket(_udp.remoteIP(), _udp.remotePort());
  // Send a reply, to the IP address and port that sent us the packet we received.
  String output = "$EYERIS," + String(millis());
  for (int i = 0; i < 4; i++)
  {
    output += "," + String(GetRelayStatus(i) ? W_ON : W_OFF);
  }

  // Add all of the internal sensors
  output += "," + readBME280();
  output += "," + readINA260();
  
  _udp.write(output.c_str());
  _udp.write("\r\n");
  _udp.endPacket();
}

void writeRS485Data() {

  _udp.beginPacket(_udp.remoteIP(), _udp.remotePort());
  // Send a reply, to the IP address and port that sent us the packet we received.
  _udp.write("$RS485,");
  //make the end of the buffer
  _rs485Buffer[rs485Index] = '\0';
  String output(_rs485Buffer);
  output.trim();
#ifdef DEBUG
  Serial.println("Write to client: " + output + ", size: " + rs485Index);
//  Serial.print(rs485Index);
//  Serial.println(" Contents: " + output);
#endif
//  _udp.write(output.c_str());
  _udp.write(_rs485Buffer);
  _udp.write("\r\n");
  _udp.endPacket();

  rs485Index = 0;
}

/**
   \brief WriteClientResponse void. Write html response.


   \return void
*/
void WriteClientResponse()
{
  _udp.beginPacket(_udp.remoteIP(), _udp.remotePort());
  // Send a reply, to the IP address and port that sent us the packet we received.
  _udp.write(PREFIX);

  if (_commandSend == CH_R)
  {
    _udp.write(CH_R);
    // Add relay data.
    for (int i = 0; i < RELAY_COUNT; i++)
    {
#ifdef DEBUG
      Serial.println("Relay " + String(GetRelayStatus(i)?1:0) );
//      IntToChars(i + 1, _buffer);
//      Serial.print(_buffer);
//      Serial.print(" ");
//      Serial.println(GetRelayStatus(i) ? W_ON : W_OFF);
#endif
      _udp.write(GetRelayStatus(i) ? CH_1 : CH_0);
    }
  }

  _udp.endPacket();
}