 /////////////////////////////////////////////////////////////////////////////////////////
// Module Name: FW_2401.ino
// Project: Foucault Pendulum, Subsystem BobControl
// Target: Arduino MEGA + ETHernet Shield
// Last Mod: See VERSIONSTRING
// jan@breem.nl,  janbee@hack42.nl, mail@foucaultpendulum.nl
// www.foucaultslinger.nl   www.foucaultpendulum.nl
//
/////////////////////////////////////////////////////////////////////////////////////////
#include "Globals.h"
#include <Arduino.h>
#include <Adafruit_INA219.h>  // for High-Side Current / Voltage Sensor

#define VERSIONSTRING "BobControl 2024-04-05"
#define VERSIONNUMBER 40405 

unsigned int VersionNumber;
bool GeneralError;

// for power management
Adafruit_INA219 Ina219; 
int BatteryVoltage_mV, BatteryCurrent_mA;
bool DoReadBatteryStatus;

// for BME280 Temperature, Hygro an Barometric pressure
#include <Adafruit_BME280.h>
Adafruit_BME280 bme;
bool BME280_IsInitialized;
int TemperatureBME, HygroBME, BaroBME;

// For Dallas Temperature Sensor(s)
#include <OneWire.h> // for Dallas Sensors
OneWire DsT (30); // pin 30, PORT C7
#define EEPROM_STARTADDRESS_DALLAS 0
#define MAX_DALLAS_SENSORS 5
byte DallasData[12];
byte DallasCodes[MAX_DALLAS_SENSORS][8];
int DallasTemperatures[MAX_DALLAS_SENSORS];
byte NumberOfDallasSensors;
bool DallasSensorsAreInitialized, ReadaDallasSensor;
byte TopTemperature;

/**************************************************************************************************/
byte EEPROM_ReadByte (unsigned int EEpromAddress)
{
  // Wait for completion of previous action
  while (EECR & (1 << EEPE));
  EEAR = EEpromAddress;
  EECR |= (1 << EERE);
  return EEDR;
}

/**************************************************************************************************/
int EEPROM_ReadInt(unsigned int EEpromAddress)
{
  int lo, hi;
  lo = EEPROM_ReadByte (EEpromAddress);
  hi = EEPROM_ReadByte (EEpromAddress + 1);
  return ((hi << 8) | lo);
}

/**************************************************************************************************/
void EEPROM_WriteByte (unsigned int EEpromAddress, byte EEpromData)
{
  // Wait for completion of previous action
  while (EECR & (1 << EEPE));
  EEAR = EEpromAddress;
  EEDR = EEpromData;
  EECR |= (1 << EEMPE);
  EECR |= (1 << EEPE);
}

/**************************************************************************************************/
void EEPROM_WriteInt (unsigned int EEpromAddress, int EEpromData)
{
  EEPROM_WriteByte (EEpromAddress, lowByte (EEpromData));
  EEPROM_WriteByte (EEpromAddress + 1, highByte (EEpromData));
}

/**************************************************************************************************/
void EEPROM_PARAMETERS_Clear (void)
{
  Serial.println ("Erasing Parameters from EEPROM");
  for (byte i=0; i<30; i++) 
    EEPROM_WriteByte (EEPROM_STARTADDRESS_PARAMETERS + i, 255); // looks as never written to
}

/**************************************************************************************************/
void WriteParametersToEEprom (void)
{
  DoWriteParametersToEEprom = false;
  Serial.println ("Write Parameters to EEPROM");
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS     , TStartLookForCenter);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS +  2, TMissedCenter);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS +  4, TStartLookForRim_1);  
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS +  6, TMissedRim_1);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS +  8, TStartLookForRim_2);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 10, TMissedRim_2);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 12, RimCoilRadius_mm);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 14, SetPoint_Amplitude_mm);

  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 16, TMidDrive);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 18, TMinimalDriveWidth);
  EEPROM_WriteInt (EEPROM_STARTADDRESS_PARAMETERS + 20, TMaximalDriveWidth);
}

/**************************************************************************************************/
void ReadParametersFromEEprom (void)
{
  TStartLookForCenter =   EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS);
  TMissedCenter =         EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS +  2);
  TStartLookForRim_1 =    EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS +  4);
  TMissedRim_1 =          EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS +  6);
  TStartLookForRim_2 =    EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS +  8);
  TMissedRim_2 =          EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 10);
  RimCoilRadius_mm =      EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 12);
  SetPoint_Amplitude_mm = EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 14); 
  TMidDrive =             EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 16);
  TMinimalDriveWidth =    EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 18);
  TMaximalDriveWidth =    EEPROM_ReadInt (EEPROM_STARTADDRESS_PARAMETERS + 20);
  Serial.println ("Done Reading from EEPROM");
  DoReportParameters = true;
}

//********************************************************************************************************
void ReportParameters (void)
{
  DoReportParameters = false; // only once
  Serial.println ("\nSettings for synchronisation");
  Serial.print ("TStartLookForCenter   "); Serial.println (TStartLookForCenter);
  Serial.print ("TMissedCenter         "); Serial.println (TMissedCenter);  
  Serial.print ("TStartLookForRim_1    "); Serial.println (TStartLookForRim_1); 
  Serial.print ("TMissedRim_1          "); Serial.println (TMissedRim_1); 
  Serial.print ("TStartLookForRim_21   "); Serial.println (TStartLookForRim_2); 
  Serial.print ("TMissedRim_2          "); Serial.println (TMissedRim_2); 
  Serial.println ("\nSettings for Drive:");
  Serial.print ("TMinimalDriveWidth    "); Serial.println (TMinimalDriveWidth); 
  Serial.print ("TMaximalDriveWidth    "); Serial.println (TMaximalDriveWidth); 
  Serial.print ("TMidDrive             "); Serial.println (TMidDrive); 
  Serial.print ("TStartDrive           "); Serial.println (TStartDrive); 
  Serial.print ("TStopDrive            "); Serial.println (TStopDrive);  
  Serial.println ("\nSettings for Amplitude Control:");
  Serial.print ("RimCoilRadius mm      "); Serial.println (RimCoilRadius_mm); 
  Serial.print ("Setpoint Amplitude mm "); Serial.println (SetPoint_Amplitude_mm); 
  Serial.println ();
}

//********************************************************************************************************
void ResetArduino (byte ResetCause)
{
  // Resetting the Arduino from software can be done by jumping to the reset vector, but
  // then the hardware will not be reset, particularly the Ethernet Shield.
  // Here we pull the RESET line LOW with a Fet, causing a complete hardware reset.
  // This requires a special circuit to stretch and invert the reset pulse. See diagram.
  // But first we write something to EEPROM, so we can find out what caused the reset.
  EEPROM_WriteByte (RESET_FLAG_ADDRESS, ResetCause);
  delay (100);
  DDRH |= 0x20;  // Data direction = Output
  PORTH |= 0x20; // PORTH bit 5 pin 8 high brings the FET in conduction after some delay.
}

//********************************************************************************************************
void ReadBatteryStatus (void) 
{
  DIAGPIN_A9_H
  DoReadBatteryStatus = false;
  BatteryVoltage_mV = round (Ina219.getBusVoltage_V() * 1000);
  BatteryCurrent_mA = -round (Ina219.getCurrent_mA()); // Show charging Positive
  DIAGPIN_A9_L
}  

//********************************************************************************************************
void ShowBatteryState (void) 
{
  ReadBatteryStatus ();
  Serial.print("Battery Voltage: "); Serial.print (BatteryVoltage_mV); Serial.print (" mV");
  Serial.print(" Charge Current: "); Serial.print (BatteryCurrent_mA); Serial.println (" mA");
  Serial.println("");
}

//********************************************************************************************************
void ShowHelp (void) 
{
  Serial.println ("\nAvailable commands: \n");
  Serial.println ("b: Show Battery State");
  Serial.println ("C: Clear EEPROM Area");
  Serial.println ("D: Set Default Parameters");
  Serial.println ("H: Show this Help info");
  Serial.println ("p: Show Parameters");
  Serial.println ("P: Read Parameters from EEPROM"); 
  Serial.println ("R: RESET ARDUINO");
  Serial.println ("T: Search Dallas Temperature Sensors");
}

//*************************************************************************************************
void EEPROM_DALLAS_Clear (void)
{
  Serial.println ("Erasing Dallas Data from EEPROM");
  for (byte i=0; i<30; i++) EEPROM_WriteByte (EEPROM_STARTADDRESS_DALLAS + i, 255); // looks as never written to
}

//****************************************************************************
void SearchDallasSensors (void) // Called from loop () when letter "T" is entered
{
  byte i, j;
  
  EEPROM_DALLAS_Clear ();
  Serial.println ("Searching Dallas Temperature Sensors");
  NumberOfDallasSensors = 0;
  while (NumberOfDallasSensors < MAX_DALLAS_SENSORS)
  {
    if (!DsT.search (DallasCodes[NumberOfDallasSensors]))
    {
      Serial.print ("Number of Dallas Sensors: ");
      Serial.print (NumberOfDallasSensors);
      Serial.println (" Found ");
      DsT.reset_search ();
      if (NumberOfDallasSensors == 0)
      {
        DallasSensorsAreInitialized = false;
        return;
      }
      
      Serial.println ("Writing Dallas Sensor Data to EEPROM");
      EEPROM_WriteByte (EEPROM_STARTADDRESS_DALLAS, NumberOfDallasSensors);
      for (i = 0; i < NumberOfDallasSensors; i++)
      {
        for (j = 0; j < 8; j++)
          EEPROM_WriteByte (EEPROM_STARTADDRESS_DALLAS + i * 8 + j + 1, DallasCodes[i][j]);
      ReadDallasSensor(i);
      }  
      return;
    }
    else
    {
      Serial.print ("Sensor no ");
      Serial.print (NumberOfDallasSensors);
      Serial.print (": ROM = ");
      for (i = 0; i < 8; i++)
      {
        Serial.print (DallasCodes[NumberOfDallasSensors][i]);
        Serial.write(' ');
      }
      if (OneWire::crc8 (DallasCodes[NumberOfDallasSensors], 7) != DallasCodes[NumberOfDallasSensors][7])
      {
        //PrintPMstr (MsgDallas_CRCnotValid);
        Serial.println ("Dallas CRC not valid");
        DallasSensorsAreInitialized = false;
        return;
      }
      switch (DallasCodes[NumberOfDallasSensors][0])
      {
        case 0x10:
           Serial.println (", Chip = DS18S20"); // or old DS1820. 0.5 K resolution
          break;
        case 0x28:
            Serial.println (", Chip = DS18B20"); // 0.0625 K resolution
          break;
        case 0x22:
            Serial.println (", Chip = DS1822");  // 0.5 K resolution
          break;
        default:
           Serial.println ("Device is not a DS18x20 family device.\n");
          return;
      }
      NumberOfDallasSensors++;
    }
  }
}

// ****************************************************************************
// This function is only called at startup from InitSensors () or 
// when a 'T' is entered.
// During normal operation we use a method where the actions are
// distributed over different timeslices.
void ReadDallasSensor (byte SensorNumber)
{
  byte i, data[12];
  float celsius;

  DsT.reset ();
  DsT.select (DallasCodes[SensorNumber]);
  DsT.write (0x44, 0);  // start conversion, with parasite power OFF at the end

  delay(1000);     // maybe 750ms is enough, maybe not

  DsT.reset ();
  DsT.select (DallasCodes[SensorNumber]);
  DsT.write (0xBE);  // Read Scratchpad
  for (i = 0; i < 9; i++) data[i] = DsT.read(); // we need 9 bytes
  int16_t raw = (data[1] << 8) | data[0];
  // DS18B20 has family code 0x28 (first number, hex) and calibration is
  // T (Celcius) = readout * 0.0625.
  // DS18S20 has family code 0x10 (first number, hex) and calibration is
  // T (Celcius) = readout * 0.5
  if (DallasCodes[SensorNumber][0] == 0x28)
    celsius = (float)raw * 0.0625;
  else if (DallasCodes[SensorNumber][0] == 0x28)
    celsius = (float)raw * 0.5;
  Serial.print ("Sensor: ");
  Serial.print (SensorNumber);
  Serial.print (": Temperature = ");
  Serial.print (celsius);
  Serial.println (" Celsius, ");
}


//********************************************************************************************************
void ReadROMcodesFromEEprom (void)  // called from Init_Sesors ()
{
  byte i, j, k;
  DallasSensorsAreInitialized = false;
  Serial.println ("Reading Dallas ROM codes from EEPROM");
  for (i = 0; i < MAX_DALLAS_SENSORS; i++)
    DallasTemperatures[i] = -999;
  NumberOfDallasSensors = EEPROM_ReadByte (EEPROM_STARTADDRESS_DALLAS);
  if (NumberOfDallasSensors > MAX_DALLAS_SENSORS)
  {
    Serial.println ("No Dallas ROM codes found."); 
    Serial.println ("Enter 'D' to search for Dallas Temperature Sensors");
    DallasSensorsAreInitialized = false;
  }
  else
  {
    DallasSensorsAreInitialized = true;
    Serial.println ("EEPROM reports: "); 
    Serial.println (NumberOfDallasSensors);
    for (i = 0; i < NumberOfDallasSensors; i++)
    {
      for (j = 0; j < 8; j++)
        DallasCodes[i][j] = EEPROM_ReadByte (EEPROM_STARTADDRESS_DALLAS + i * 8 + j + 1);
      Serial.print ("Sensor: ");  
      Serial.print (i);
      Serial.print (": ROM = ");
      for (k = 0; k < 8; k++)
      {
        Serial.print (DallasCodes[i][k]);
        Serial.print(' ');
      }
      ReadDallasSensor(i);
    }
    Serial.println ("Enter 'D' to search again for Dallas Temperature Sensors");
  }
}

//********************************************************************************************************
void setup (void)
{
  Serial.begin (19200);
  Serial.println ("\n\n\n\n\n\n\n\n\n");
  Serial.println (VERSIONSTRING);
  VersionNumber = VERSIONNUMBER;
  Serial.print ("Version Number: ");
  Serial.println (VersionNumber);  
  Serial.println();

  // Find out if we woke up from a self induced reset
  byte ResetCause = EEPROM_ReadByte (RESET_FLAG_ADDRESS);
  Serial.print ("Reset Cause: ");
  Serial.println (ResetCause);
  if (ResetCause == 0) DidResetMyself = true;
  EEPROM_WriteByte (RESET_FLAG_ADDRESS, 255); // now it looks as never touched
  
  // I/O's
  DDRA |= 0x05;  // HOLD and CHRG
  PORTA|= 0xAA;  // Activate Internal Pullup's for Jumper Block.

  CHARGEBATTERY_ON
  HOLDPOWER_ON

  ReadParametersFromEEprom ();
  ReportParameters ();

  ReadROMcodesFromEEprom (); // for Dallas Temperature Sensors
  
  Init_BobControl ();
  Init_Messages ();
  
#ifdef USE_INA219
  // Initialize the high-side current sensor INA219
  // By default the initialization will use the largest range (32V, 2A).
  if (!Ina219.begin()) Serial.println ("Failed to find INA219 chip");
  // Use a lower 16V, 400mA range (higher precision on volts and amps):
  Ina219.setCalibration_16V_400mA();
#endif

  Serial.print ("Initialize BME280 sensor for Temperature, Hygro and Baro.....");
  BME280_IsInitialized = true;
  if (!bme.begin())
  {
    Serial.println ("\nCould not find a valid BME280 sensor, check wiring!\n");
    BME280_IsInitialized = false;
  }
  else Serial.println ("OK");
  
  USEDIAGPIN_A8  // Message in / out
  USEDIAGPIN_A9  // Reading battery voltage and current
  USEDIAGPIN_A10
  USEDIAGPIN_A11
  USEDIAGPIN_A12 // PassRim_2
  USEDIAGPIN_A13 // PassRim_1
  USEDIAGPIN_A14 // PassCenter
  USEDIAGPIN_A15 // 10 kHz Timer 1 interrupt handler InService time
}

// ********************************************************************************************************
void loop(void)
{
  Update_Messages (); 
 
  char SerIn[2];
  if (Serial.available())
  {
    Serial.readBytes (SerIn, 2);
    if (SerIn[0] == 'b') ShowBatteryState ();
    if (SerIn[0] == 'C') EEPROM_PARAMETERS_Clear ();
    if (SerIn[0] == 'D') SearchDallasSensors (); // also clears and rewrites EEPROM area
    if (SerIn[0] == 'h') ShowHelp ();
    if (SerIn[0] == 'P') ReadParametersFromEEprom ();
    if (SerIn[0] == 'p') DoReportParameters = true;
    if (SerIn[0] == 'R') ResetArduino (2);  
  } 

  if (DoReportParameters) ReportParameters();
  if (DoWriteParametersToEEprom) WriteParametersToEEprom ();
  if (DoUpdateAmplitudeControl) UpdateAmplitudeControl();
  if (DoReadBatteryStatus) ReadBatteryStatus(); 

  if (ReadaDallasSensor || OneSecondPassed)
  {
    static byte DallasSensorNumber;
    int raw;
    float Celsius;
    ReadaDallasSensor= false;
    OneSecondPassed = false;
    // First read a sensor;
    //Serial.print ("Read Dallas Sensor no: ");
    //Serial.println (DallasSensorNumber);
    DsT.reset ();
    DsT.select (DallasCodes[DallasSensorNumber]);
    DsT.write (0xBE); // read Scratchpad
    for (byte i = 0; i < 9; i++) DallasData[i] = DsT.read (); // we need 9 bytes
    raw = (DallasData[1] << 8) | DallasData[0];
    if (DallasCodes[DallasSensorNumber][0] == 0x28)
    Celsius = raw * 0.0625;
    else if (DallasCodes[DallasSensorNumber][0] == 0x10)
      Celsius = raw * 0.5;
    if (DallasSensorNumber < NumberOfDallasSensors)
      DallasTemperatures[DallasSensorNumber] = round (Celsius * 10); // decidegrees
    else
      DallasTemperatures[DallasSensorNumber] = -999;
    //Serial.print ("Temperature of Dallas Sensor: ");
    //Serial.println (DallasTemperatures[DallasSensorNumber]);
    // prepare for OutMessage
    int T = DallasTemperatures[0]; // decidegrees
    if (T >= 0) TopTemperature = T / 10; else TopTemperature = 100 + T / 10;
    if (T == 999) TopTemperature = 255;  
      
    // Then start the conversion of the next sensor    
    if (++DallasSensorNumber >= NumberOfDallasSensors) DallasSensorNumber = 0;
    //Serial.print ("Start conversion for Dallas Sensor no: ");
    //Serial.println (DallasSensorNumber);
    DsT.reset ();
    DsT.select (DallasCodes[DallasSensorNumber]);
    DsT.write (0x44, 1);  // start conversion command
  }

  if (DoReadBME )
  {
    DoReadBME = false;
    if (BME280_IsInitialized)
    {
      TemperatureBME = round (bme.readTemperature () * 10); // deci degrees C
      HygroBME = round (bme.readHumidity ()); // % relative
      BaroBME = round (bme.readPressure() / 10); // mBar = HectoPascal
    }
    else
    {
      TemperatureBME = -999;
      HygroBME = -999;
      BaroBME = -999;
    }
    char Str[40];
    sprintf (Str, "T:%4d Hy:%3d Ba:%6d", TemperatureBME, HygroBME, BaroBME);
    Serial.println (Str);
  }
  
  if (false)  // to check proper working of A/D conversions
  {
    char Str[40];
    sprintf (Str, "%3d %3d %3d %3d   %3d %3d",
      AdcHallWest, AdcHallEast, AdcHallNorth, AdcHallSouth, AdcCenter, AdcRim);
    Serial.println (Str);
    delay (100);
  }  
}
