/*
 Test operation of Wireless RF Shaft Encoder.  On the remote side of the RF link, 
 the rotating sensor is based on SparkFun's LIS3DH digital XYZ accelerometer 
 connected via SPI to a Low Power Labs Moteino M0 with a RFM69HCW radio all 
 powered by a battery with an analog battery voltage monitor.  On the local 
 (stationary) side a Moteino M0 with RFM69HCW radio and a Kent Engine DAS shield 
 with RLS shaft encoder (and pressure sensors, optional for this test) all 
 powerd by the USB link to a PC.

 Expects the analog RLS shaft encoder on the flame licker engine to be 
 connected to an analog input pin on the locaL side so that it can be sampled 
 and compared to the data received from the wireless sensor.
 
 In DEV mode, the data from both THE REMOTE AND LOCAL sensors are streamed 
 out the Serial port to the Arduino IDE in human readable form for inspection.
 Then, in RUN mode, only the binary data is passed up the serial link for real-time 
 processing or logging by the PC.
  
 Copyright (c) 2019-2020 John David Heinzmann 
*/

// The run mode and appropriate radio packet payload struct used on both sides of 
// the radio link is defined in one place.  To change run mode, speciy RUN_MODE 
// there and then upload sketches for both sides.  Edit/save the file here:  
// C:\Users\User\Documents\Arduino\libraries\RF_ShaftEnc_Common
#include "RF_ShaftEnc_Common.h"

// Output modes for sending data to PC
#define CSV      (0)
#define BINARY   (1) // needs to be coded, still
#define OUT_MODE (CSV) // CSV or BINARY output to USB?

// data byte counts from remote sensor
#define SAMPS_PER_SCAN   (4) // Time, 2 accels, bat voltage
#define BYTES_PER_SAMP   (2) // samples are unsigned wich are 2 bytes each
#define BYTES_PER_SCAN   (SAMPS_PER_SCAN * BYTES_PER_SAMP) 

#define RLS_ANGLE_PIN    (A1) // analog input pin to which RLS encoder Vout is connected

// global variables
byte dataBytes[BYTES_PER_SCAN]; // byte array to receive remote data into
unsigned int rawRLS_Angle; // raw ADC counts from RLS analog shaft encoder

// **********************************************************************************
// This is taken from the global portion of the Struct_receive.INO RFM69 Example 
// **********************************************************************************
// Copyright Felix Rusu 2018, http://www.LowPowerLab.com/contact
// **********************************************************************************
// License
// **********************************************************************************
// This program is free software; you can redistribute it 
// and/or modify it under the terms of the GNU General    
// Public License as published by the Free Software       
// Foundation; either version 3 of the License, or        
// (at your option) any later version.                    
//                                                        
// This program is distributed in the hope that it will   
// be useful, but WITHOUT ANY WARRANTY; without even the  
// implied warranty of MERCHANTABILITY or FITNESS FOR A   
// PARTICULAR PURPOSE. See the GNU General Public        
// License for more details.                              
//                                                        
// Licence can be viewed at                               
// http://www.gnu.org/licenses/gpl-3.0.txt
//
// Please maintain this license information along with authorship
// and copyright notices in any redistribution of this code
// **********************************************************************************
#include <RFM69.h>         //get it here: https://www.github.com/lowpowerlab/rfm69
#include <RFM69_ATC.h>     //get it here: https://www.github.com/lowpowerlab/rfm69
#include <SPIFlash.h>      //get it here: https://www.github.com/lowpowerlab/spiflash
#include <SPI.h>           //included with Arduino IDE install (www.arduino.cc)
//*********************************************************************************************
//************ IMPORTANT SETTINGS - YOU MUST CHANGE/CONFIGURE TO FIT YOUR HARDWARE *************
//*********************************************************************************************
#define NODEID      1
#define NETWORKID   100
//Match frequency to the hardware version of the radio on your Moteino (uncomment one):
//#define FREQUENCY     RF69_433MHZ
//#define FREQUENCY     RF69_868MHZ
#define FREQUENCY     RF69_915MHZ
#define ENCRYPTKEY    "sampleEncryptKey" //has to be same 16 characters/bytes on all nodes, not more not less!
#define IS_RFM69HW_HCW  //uncomment only for RFM69HW/HCW! Leave out if you have RFM69W/CW!
//*********************************************************************************************
//Auto Transmission Control - dials down transmit power to save battery
//Usually you do not need to always transmit at max output power
//By reducing TX power even a little you save a significant amount of battery power
//This setting enables this gateway to work with remote nodes that have ATC enabled to
//dial their power down to only the required level
#define ENABLE_ATC    //comment out this line to disable AUTO TRANSMISSION CONTROL
//*********************************************************************************************
#if defined (MOTEINO_M0) && defined(SERIAL_PORT_USBVIRTUAL)
  #define Serial SERIAL_PORT_USBVIRTUAL // Required for Serial on Zero based boards
#endif

#ifdef ENABLE_ATC
  RFM69_ATC radio;
#else
  RFM69 radio;
#endif

// SPIFlash flash(SS_FLASHMEM, 0xEF30); //EF40 for 16mbit windbond chip
bool promiscuousMode = false; //set to 'true' to sniff all packets on the same network

Payload RcvdRFDataPkt;
// ********************************************************************************
// End of global portion of code taken from Felix's Struct_receive.ino example
// ********************************************************************************

//*****************************************************************************************************************************
// From Felix's Target.ino example
#define BR_300KBPS             //run radio at max rate of 300kbps!
//*****************************************************************************************************************************

//#define TIMING_PIN              (2) // digital pin for probing accel read timing
#define TIMING_PIN      LED_BUILTIN // digital pin for probing accel read timing (pin 13)

char PrintStr[100]; // For messages out the USB port

void setup()
{
  SerialUSB.begin(10);   // Begin the serial monitor serial connection (baud rate ignored)
  // while (!Serial); // wait for serial port to connect. Needed for native USB port only  

  // *******************************************************************************
  // Taken (and modified) from setup() portion of Felix's Struct_receive.ino example.
  // *******************************************************************************
  radio.initialize(FREQUENCY,NODEID,NETWORKID);
  #ifdef IS_RFM69HW_HCW
    radio.setHighPower(); //must include this only for RFM69HW/HCW!
  #endif
  //radio.encrypt(ENCRYPTKEY);
  radio.promiscuous(promiscuousMode);

  SerialUSB.println("Wireless Stationary Shaft Encoder - Local Moteino M0 monitoring remote");
  SerialUSB.println("encoder via radio, and local RLS encoder directly.");
  sprintf(PrintStr, "\nRFM96HCW radio Listening at %d Mhz...\n", FREQUENCY==RF69_433MHZ ? 433 : FREQUENCY==RF69_868MHZ ? 868 : 915);
  SerialUSB.println(PrintStr);
//  if (flash.initialize())
//    SerialUSB.println("SPI Flash Init OK!");
//  else
//    SerialUSB.println("SPI Flash Init FAIL! (is chip present?)");
  // *******************************************************************************
  // End of setup() portion of code taken from Felix's Struct_receive.ino  
  // *******************************************************************************

  //*********************************************************************************************
  // Taken from Felix's RFM69/Examples/WirelessProgramming_OTA/Target/Target.ino on GitHub
  //*********************************************************************************************
  #ifdef BR_300KBPS
  radio.writeReg(0x03, 0x00);  //REG_BITRATEMSB: 300kbps (0x006B, see DS p20)
  radio.writeReg(0x04, 0x6B);  //REG_BITRATELSB: 300kbps (0x006B, see DS p20)
  radio.writeReg(0x19, 0x40);  //REG_RXBW: 500kHz
  radio.writeReg(0x1A, 0x80);  //REG_AFCBW: 500kHz
  radio.writeReg(0x05, 0x13);  //REG_FDEVMSB: 300khz (0x1333)
  radio.writeReg(0x06, 0x33);  //REG_FDEVLSB: 300khz (0x1333)
  radio.writeReg(0x29, 240);   //set REG_RSSITHRESH to -120dBm
  #endif
  //*********************************************************************************************
  // End of snip from Felix's Target.ino example
  //*********************************************************************************************

  delay(2000);
  if ( (RUN_MODE == DEV) || (RUN_MODE == CAL) )
  {
    SerialUSB.println("remTime_ms, X, Y, rawBatV, locTime_ms, rawRLS_Angle, remTheta, TDC");
  }
  else if (RUN_MODE == RUN)
  {
    SerialUSB.println("remTime_ms, remAngle, rawBatV, TDC, locTime_ms, rawRLS_Angle");
  }

  analogWriteResolution(10); // Set the DAC resolution to 10 bits
  
  pinMode(TIMING_PIN, OUTPUT);
}

void loop()
{
  if (radio.receiveDone())
  {
    digitalWrite(TIMING_PIN, HIGH);
    unsigned long locTime_ms = millis();
    rawRLS_Angle = analogRead(RLS_ANGLE_PIN); // read RLS encoder angle
    if (radio.DATALEN != sizeof(Payload))
      SerialUSB.print("Invalid payload received, not matching Payload struct!");
    else
    {
      RcvdRFDataPkt = *(Payload*)radio.DATA; //assume radio.DATA actually contains our struct and not something else
    }
    
    // write local angle out to DAC
    float DAC_out = RcvdRFDataPkt.theta * 1023/(2.0*PI);
    analogWrite(A0, DAC_out); // Set the DAC to theta scaling 0 to 2*PI radians to 0 to 1023 counts
    
    if (radio.ACKRequested())
    {
      byte theNodeID = radio.SENDERID; // not used?
      radio.sendACK();
    }

    if (RcvdRFDataPkt.theta < 0)
    {
      RcvdRFDataPkt.theta += 2*PI;
    }
    
    #if ( (RUN_MODE == DEV) || (RUN_MODE == CAL) )
      sprintf(PrintStr, "%d,%1.3f,%1.3f,%1.2f,%d,%d,%1.3f,%d",
                        RcvdRFDataPkt.remTime_ms, RcvdRFDataPkt.X, 
                        RcvdRFDataPkt.Y, RcvdRFDataPkt.batV, locTime_ms, 
                        rawRLS_Angle, RcvdRFDataPkt.theta, RcvdRFDataPkt.TDC);
      SerialUSB.println(PrintStr);
    #elif (RUN_MODE == RUN)
      #if (OUT_MODE == CSV)
        sprintf(PrintStr, "%d,%1.3f,%1.3f,%d,%d,%d",
                          RcvdRFDataPkt.remTime_ms, RcvdRFDataPkt.theta, RcvdRFDataPkt.batV, 
                          RcvdRFDataPkt.TDC, locTime_ms, rawRLS_Angle);
        SerialUSB.println(PrintStr);
      #elif (OUT_MODE == BINARY)
        SerialUSB.write(dataBytes, BYTES_PER_SCAN);
        // pass also angle data read from the analog sensor to Serial port
        SerialUSB.write((byte)(locTime_ms>>8)); // MSB
        SerialUSB.write((byte)((locTime_ms<<8)>>8)); // LSB
        SerialUSB.write((byte)(rawRLS_Angle>>8)); // MSB
        SerialUSB.write((byte)((rawRLS_Angle<<8)>>8)); // LSB
        #endif // OUT_MODE
    #endif // RUN_MODE
      
    digitalWrite(TIMING_PIN, LOW);
  }
}

void Blink(byte PIN, int DELAY_MS)
{
  pinMode(PIN, OUTPUT);
  digitalWrite(PIN,HIGH);
  delay(DELAY_MS);
  digitalWrite(PIN,LOW);
}