 /////////////////////////////////////////////////////////////////////////////////////////
// Module Name: Messages.cpp
// Project: Foucault Pendulum, Subsystem BobControl
// Target: Arduino MEGA + ETHernet Shield
// Last Mod: See .ino file
// jan@breem.nl,  janbee@hack42.nl, mail@foucaultpendulum.nl
// www.foucaultslinger.nl   www.foucaultpendulum.nl
//
/////////////////////////////////////////////////////////////////////////////////////////

#include "Globals.h"
#include <Ethernet.h>
#include <EthernetUdp.h>

// for Ethernet communication
EthernetUDP Udp;
byte mac[] = {0x90, 0xA2, 0xDA, 0x0F, 0x67, 50};
IPAddress ArduinoIP (192, 168, 2, 50);
unsigned int ArduinoPort = 8888;

// for InMessage:
#define INMESSAGESIZE 30
byte InMessageBuffer[INMESSAGESIZE];
unsigned int
  Command, 
  TStartLookForCenter, TMissedCenter,
  TStartLookForRim_1,  TMissedRim_1, 
  TStartLookForRim_2,  TMissedRim_2, 
  RimCoilRadius_mm,    SetPoint_Amplitude_mm,
  TMidDrive; 
byte
  TMinimalDriveWidth, TMaximalDriveWidth;
bool
  ForceMinimalDriveWidth, ForceMaximalDriveWidth,
  UseMinimalDriveWidth, UseMaximalDriveWidth, 
  EnableDrive, ForceReSync, HoldPower, ChargeBattery,
  DoReportParameters, DoWriteParametersToEEprom;
  
// for OutMessage:
#define OUTMESSAGESIZE 40
byte OutMessageBuffer[OUTMESSAGESIZE + 1];  // payload + 1 extra for length byte
unsigned int Status;

// ********************************************************************************************************
void DecodeInMessage (void)
{
  // Copy values from messagebuffer 
  memcpy(&Command,               &InMessageBuffer[ 0], 2);
  memcpy(&TStartLookForCenter,   &InMessageBuffer[ 2], 2);
  memcpy(&TMissedCenter,         &InMessageBuffer[ 4], 2);
  memcpy(&TStartLookForRim_1,    &InMessageBuffer[ 6], 2);
  memcpy(&TMissedRim_1,          &InMessageBuffer[ 8], 2);
  memcpy(&TStartLookForRim_2,    &InMessageBuffer[10], 2);
  memcpy(&TMissedRim_2,          &InMessageBuffer[12], 2);
  memcpy(&RimCoilRadius_mm,      &InMessageBuffer[14], 2);
  memcpy(&SetPoint_Amplitude_mm, &InMessageBuffer[16], 2);
  memcpy(&TMidDrive,             &InMessageBuffer[18], 2);
  TMaximalDriveWidth =            InMessageBuffer[20];
  TMinimalDriveWidth =            InMessageBuffer[21];
  // InMessageBuffer[22 and on] currently not used
  
  // Process values
  if ((Command & 0x0004) == 0x0004) HalfSwing = !HalfSwing;
  ForceReSync =            ((Command & 0x0008) == 0x0008);
  HoldPower =              ((Command & 0x0010) == 0x0010);
  ChargeBattery =          ((Command & 0x0020) == 0x0020);
  ForceMinimalDriveWidth = ((Command & 0x0100) == 0x0100);
  ForceMaximalDriveWidth = ((Command & 0x0200) == 0x0200);
  EnableDrive =            ((Command & 0x0800) == 0x0800);
  if ((Command & 0x1000) == 0x1000) DidResetMyself = DidReSyncMyself = false;
  if ((Command & 0x4000) == 0x4000) 
  {
    DoWriteParametersToEEprom = true; 
    DoReportParameters = true;
  }
  if ((Command & 0x8000) == 0x8000) ResetArduino (1); 
  if (HoldPower) HOLDPOWER_ON else HOLDPOWER_OFF 
  if (ChargeBattery) CHARGEBATTERY_ON else CHARGEBATTERY_OFF
}

// ********************************************************************************************************
void PrepareOutMessage(void)
{
  // Prepare values
  OutMessageBuffer[0] = OUTMESSAGESIZE; // string length on the Pascal side
  Status = 0;

  if (SeenCenter)   Status |= 0x0001; SeenCenter = false; // only once
  if (MissedCenter) Status |= 0x0002; MissedCenter = false;
  if (HalfSwing)    Status |= 0x0004;
  if (HaveSync)     Status |= 0x0008;
  
  if (SeenRim_1)    Status |= 0x0010; SeenRim_1 = false;
  if (MissedRim_1)  Status |= 0x0020; MissedRim_1 = false;
  if (SeenRim_2)    Status |= 0x0040; SeenRim_2 = false;
  if (MissedRim_2)  Status |= 0x0080; MissedRim_2 = false;
  
  if (EnableDrive && UseMinimalDriveWidth) Status |= 0x0100;
  if (EnableDrive && UseMaximalDriveWidth) Status |= 0x0200;

  if (DidResetMyself)  Status |= 0x1000;
  if (DidReSyncMyself) Status |= 0x2000; 
  if (GeneralError)    Status |= 0x8000;
  
  // Copy values to messagebuffer
  memcpy (&OutMessageBuffer[1],  &VersionNumber, 2);
  memcpy (&OutMessageBuffer[3],  &Status, 2);
  memcpy (&OutMessageBuffer[5],  &TPassCenter, 2);
  memcpy (&OutMessageBuffer[7],  &TPassRim_1, 2);
  memcpy (&OutMessageBuffer[9],  &TPassRim_2, 2);
  memcpy (&OutMessageBuffer[11], &AmplitudeFromRim_mm, 2);
  OutMessageBuffer[13] = AdcCenter;
  OutMessageBuffer[14] = APeakCenter;
  OutMessageBuffer[15] = AdcRim;
  OutMessageBuffer[16] = APeakRim_1;
  OutMessageBuffer[17] = APeakRim_2;
  OutMessageBuffer[18] = AdcHallNorth;
  OutMessageBuffer[19] = AdcHallSouth;
  OutMessageBuffer[20] = AdcHallEast;
  OutMessageBuffer[21] = AdcHallWest;
  OutMessageBuffer[22] = AdcV24;
  memcpy (&OutMessageBuffer[23], &BatteryVoltage_mV, 2);
  memcpy (&OutMessageBuffer[25], &BatteryCurrent_mA, 2);
  OutMessageBuffer[27] = TopTemperature;
  memcpy (&OutMessageBuffer[28], &PositionCounter, 2);
  memcpy (&OutMessageBuffer[30], &TemperatureBME, 2);
  memcpy (&OutMessageBuffer[32], &HygroBME, 2);
  memcpy (&OutMessageBuffer[34], &BaroBME, 2);
  
  // OutMessageBuffer[29 and on] currently not used
}

//********************************************************************************************************
void Update_Messages (void) // called frequently from loop()
{
  int PacketSize = Udp.parsePacket();
  if (PacketSize == INMESSAGESIZE)
  {
    DIAGPIN_A8_H 
    Udp.read (InMessageBuffer, INMESSAGESIZE); 
    DecodeInMessage ();
    PrepareOutMessage ();
    Udp.beginPacket (Udp.remoteIP (), Udp.remotePort ());
    Udp.write (&OutMessageBuffer[0], OUTMESSAGESIZE + 1); // one larger than the payload
    Udp.endPacket();
    DIAGPIN_A8_L 
  } 
}

//********************************************************************************************************
void Init_Messages (void) // called at startup from setup()
{
  Serial.println ("\nInitializing UDP Communication.");
  Serial.print ("Arduino IP = ");
  Serial.print (ArduinoIP);
  Serial.print (" Port = ");
  Serial.println (ArduinoPort);
 
  Ethernet.begin (mac, ArduinoIP);
  bool success = Udp.begin (ArduinoPort);
  Serial.print ("UDP setup: ");
  if (success) Serial.println ("OK"); else Serial.println ("Failed");
}
 
