/* 
  Jeti Sensor EX Telemetry C++ Library
  
  JetiExProtocol - EX protocol implementation
  --------------------------------------------------------------------
  
  Copyright (C) 2015, 2016 Bernd Wokoeck

  - Thanks to "Jeti telemetry" pioneers who published their code:
        Uwe Gartmann, Alexander Buschek, Denis Artru, Rainer Schlosshan, Henning Stoecklein
  
  Version history:
  0.90   11/22/2015  created
  0.92   12/14/2015  15 sensor values now, data type 22b added
  0.93   12/14/2015  bug with buffer length calc for type 22b removed
  0.95   12/23/2015  Refactoring, data types GPS, date/time, 6b, 30b added
  0.96   02/21/2016  comPort number as parameter for Teensy
                     sensor device id as optional parameter (SetDeviceId())
  0.97   02/26/2016  runs w/o EX sensors (Jetibox only)
  0.98   04/09/2016  slightly improved sensor packet size calculation (avoids "flickering" values)
  0.99   06/03/2016  support for more than 15 sensors, set MAX_SENSORS in JetiExPotocol.h
                     bug in encoding of type_6b removed (thanks to AlexM_1977)
  1.00   01/29/2017  Some refactoring:
                     - Bugixes for Jetibox keys and morse alarms (Thanks to Ingmar !)
                     - Optimized half duplex control for AVR CPUs in JetiExHardwareSerialInt class (for improved Jetibox key handling)
                     - Reduced size of serial transmit buffer (128-->64 words) 
                     - Changed bitrates for serial communication for AVR CPUs (9600-->9800 bps)
                     - EX encryption removed, as a consequence: new manufacturer ID: 0xA409
                       *** Telemetry setup in transmitter must be reconfigured (telemetry reset) ***
                     - Delay at startup increased (to give receiver more init time)
                     - New HandleMenu() function in JetiexSensor.ini (including more alarm test)
                     - JETI_DEBUG and BLOCKING_MODE removed (cleanup)
  1.02   03/28/2017  New sensor memory management. Sensor data can be located in PROGMEM
  1.03   07/14/2017  Send dictionary already in serial initialization for the 1st time
                     in order to improve behaviour on telemetry reset
  1.04   07/18/2017  dynamic sensor de-/activation
  1.05   11/12/2017  send 3 textframes before start of EX transmission to get transmitter ready

  Todo:
  - better check for ex buffer overruns

  Hints:
  - http://j-log.eu/forum/viewtopic.php?p=8501#p8501
    
  Permission is hereby granted, free of charge, to any person obtaining
  a copy of this software and associated documentation files (the "Software"),
  to deal in the Software without restriction, including without limitation
  the rights to use, copy, modify, merge, publish, distribute, sublicense,
  and/or sell copies of the Software, and to permit persons to whom the
  Software is furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in
  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  IN THE SOFTWARE.

**************************************************************/

#include "JetiExProtocol.h"


// JetiSensor work data
///////////////////////
JetiSensor::JetiSensor( int arrIdx, JetiExProtocol * pProtocol )
{
  // sensor state
  m_bActive = PBIT_TST(pProtocol->SensorActivMsk, arrIdx);
  if( !m_bActive )
    return;

  // copy constant data 
  JetiSensorConst constData;
  memcpy_P( &constData, &pProtocol->m_pSensorsConst[ arrIdx ], sizeof(JetiSensorConst) );

  m_id = constData.id;
  // copy to combined sensor/value buffer
  copyLabel( (const uint8_t*)constData.text, (const uint8_t*)constData.unit, m_label, sizeof( m_label ), &m_textLen, &m_unitLen );

  // value
  m_value = pProtocol->m_pValues[ arrIdx ].m_value;

  // 0...2 decimal places
  switch( constData.precision )
  {
  case 1:  m_precision = 0x20; break; 
  case 2:  m_precision = 0x40; break; 
  default: m_precision = 0x00; break; 
  }

  // set needed space in EX frame buffer
  m_dataType = constData.dataType; 
  switch( m_dataType )
  {
  case TYPE_6b:  m_bufLen = 2; break;  //  1 byte id and data type + 1 byte value (incl. sign and prec)
  case TYPE_14b: m_bufLen = 3; break;  //  1 byte id and data type + 2 byte value (incl. sign and prec)
  case TYPE_22b: m_bufLen = 4; break;  //  1 byte id and data type + 3 byte value (incl. sign and prec)
  case TYPE_DT:  m_bufLen = 4; break;  //  1 byte id and data type + 3 byte value
  case TYPE_30b: m_bufLen = 5; break;  //  1 byte id and data type + 4 byte value
  case TYPE_GPS: m_bufLen = 5; break;  //  1 byte id and data type + 4 byte value
  }
}

// JetiExProtocol
/////////////////
void JetiExProtocol::Start( const char *SensName, uint8_t _DevIdx, JETISENSOR_CONST * pSensorArray, uint8_t SensCnt, uint8_t *SensActivMsk, enComPort comPort )
{
  SensorActivMsk  = SensActivMsk;
  m_pSensorsConst = pSensorArray;
  m_nSensors      = SensCnt;
  m_devIdLow      = DEVICE_ID_LOW+DevIdx;
  m_devIdHi       = DEVICE_ID_HI;

  // sensor name
  m_name = SensName;
  m_nameLen = strlen( m_name );
  if( _DevIdx )
    {
    DevIdx = '0' | _DevIdx;
    m_nameLen++;                            // name inkl. beim Senden angehängter DevId
    }

  // init sensor value array
  m_pValues = new JetiValue[ m_nSensors ];

  // init serial port 
  m_pSerial = JetiExSerial::CreatePort( comPort );
  m_pSerial->Init(); 

  TmoDoSend = 1;                            // Schnellstmöglich den 1. Frame senden
}

uint8_t JetiExProtocol::GetJetiboxKey()
{
  // key handling
  return m_pSerial->Getchar(); 
}


void JetiExProtocol::Tim10mS()
{
  // send every 150 ms only
  if( TmoDoSend && (--TmoDoSend == 0) )
  {
    TmoDoSend = TMO_DO_SEND;

    // navigator exit
//    if( m_bExitNav )
//    {
//      SendJetiboxExit();
//      m_bExitNav = false;
//    }
    // morse alarm
//    else if( m_alarmChar )
//    {
//      SendJetiAlarm( m_alarmChar );
//      m_alarmChar = 0;
//    }
    // EX frame...
//    else if( m_pSensorsConst )
    if( m_pSensorsConst )
    {
      static uint8_t TmoInit;

      SendExFrame( m_frameCnt++ );
      // 32 Sensorwerte + 32 Namen + SensorName = 65*150mS = 9.75 Sek. für einen Durchlauf
      if( TmoInit < (20000/150) )                 // die ersten 20 Sek. Sensorname und Dictionary mit senden, min. eine Wiederholung erzwingen
        {
        TmoInit++;
        if( (m_frameCnt/2) > m_nSensors )         // Namen werden nur jeden 2. Frame gesendet, abwechselnd mit Value
          m_frameCnt = 0;
        }
    }

    // followed by "simple text" frame
    SendJetiboxTextFrame();
  }
}

void JetiExProtocol::SetSensorValue( uint8_t id, int32_t value )
{
  if( m_pValues )
    m_pValues[id].m_value = value;
}

void JetiExProtocol::SetSensorValueGPS( uint8_t id, bool bLongitude, float value )
{
  // Jeti doc: If the lowest bit of a decimal point (Bit 5) equals log. 1, the data represents longitude. According to the highest bit (30) of a decimal point it is either West (1) or East (0).
  // Jeti doc: If the lowest bit of a decimal point (Bit 5) equals log. 0, the data represents latitude. According to the highest bit (30) of a decimal point it is either South (1) or North (0).
  // Byte 0: lo of minute, Byte 1: hi of minute, Byte 2: lo von degree, Byte 3: hi of degree 
  union
  {
    int32_t vInt;
    char    vBytes[4];
  } gps;

  // i.e.:
  // E 11� 33' 22.176" --> 11.55616 --> 11� 33.369' see http://www.gpscoordinates.eu/convert-gps-coordinates.php
  // N 48� 14' 44.520" --> 48.24570 --> 48� 14.742'
  float deg, frac = modff( value, &deg );
  uint16_t deg16 = (uint16_t)fabs( deg );
  uint16_t min16 = (uint16_t)fabs( frac * 0.6f * 100000 );
  gps.vInt = 0;
  gps.vBytes[0]  = min16 & 0xFF;
  gps.vBytes[1]  = ( min16 >> 8 ) & 0xFF;
  gps.vBytes[2]  = deg16 & 0xFF;                      // degrees 0..255
  gps.vBytes[3]  = ( deg16 >> 8 ) & 0x01;             // degrees 256..359
  gps.vBytes[3] |= bLongitude  ? 0x20 : 0;
  gps.vBytes[3] |= (value < 0) ? 0x40 : 0;
  
  SetSensorValue( id, gps.vInt );
}

void JetiExProtocol::SetSensorValueDate( uint8_t id, uint8_t day, uint8_t month, uint16_t year )
{
  // Jeti doc: If the lowest bit of a decimal point equals log. 1, the data represents date
  // Jeti doc: (decimal representation: b0-7 day, b8-15 month, b16-20 year - 2 decimals, number 2000 to be added).
  // doc seems to be wrong, this is working: b0-b7 year, b16-b20: day
  union
  {
    int32_t vInt;
    char    vBytes[4];
  } date;

  if( year >= 2000 )
    year -= 2000;

  date.vInt = 0;
  date.vBytes[0]  = year;
  date.vBytes[1]  = month;
  date.vBytes[2]  = day & 0x1F;
  date.vBytes[2] |= 0x20;
  
  SetSensorValue( id, date.vInt );
}

void JetiExProtocol::SetSensorValueTime( uint8_t id, uint8_t hour, uint8_t minute, uint8_t second )
{
  // If the lowest bit of a decimal point equals log. 0, the data represents time
  // (decimal representation: b0-7 seconds, b8-15 minutes, b16-20 hours).
  union
  {
    int32_t vInt;
    char    vBytes[4];
  } date;

  date.vInt = 0;
  date.vBytes[0]  = second;
  date.vBytes[1]  = minute;
  date.vBytes[2]  = hour & 0x1F;
  
  SetSensorValue( id, date.vInt );
}

void JetiExProtocol::SetJetiboxText( enLineNo lineNo, const char* text )
{
  #if HW_TERM == 1
  if( lineNo == 0 ) 
    TermPutTxt("JB1 ");
  else
    TermPutTxt("JB2 ");
  TermPutln(text);
  return;
  #endif


  char *pStart;
  switch( lineNo )
  {
  default:
  case 0: pStart = m_textBuffer;      break;
  case 1: pStart = m_textBuffer + 16; break;
  }
  
  bool bPadding = false;
  for( int i = 0; i < 16; i++ )
  {
    if( text[ i ] == '\0' )
      bPadding = true;

    if( !bPadding )
      pStart[ i ] = text[ i ];
    else
      pStart[ i ] = ' ';
  }
}

void JetiExProtocol::SendJetiboxTextFrame()
{
  int i;

  // send 34 byte  text message
  m_pSerial->Send( 0xFE, false );
  if( m_textBuffer[ 0 ] != '\0' )
  {
    for( i = 0; i < 32; i++)
      m_pSerial->Send( m_textBuffer[ i ], true );
  }
  // send empty message
  else
  {
    for( i = 0; i < 32; i++)
      m_pSerial->Send( 0, true );
  }
  m_pSerial->Send( 0xFF, false );    
}

//void JetiExProtocol::SendJetiboxExit()
//{
//  m_pSerial->Send( 0x7E, false );
//  m_pSerial->Send( 0x91, true );
//  m_pSerial->Send( 0x31, true );
//}

//void JetiExProtocol::SendJetiAlarm( char code ) // upper case character produces sound, lower case is silent
//{
//  bool bSound = true;
//  if( islower( code ) )
//  {
//    code = toupper( code );
//    bSound = false;
//  }
//
//  m_pSerial->Send( 0x7E, false );                      
//  m_pSerial->Send( 0x02, true );                         // length
//  m_pSerial->Send( 0x22 | (bSound ? 0x01 : 0x00), true); // alarm type "vario" w/o sound or "normal"
//  m_pSerial->Send( code, true );                         // send "morse code" character
//}


void JetiExProtocol::SendExFrame( uint8_t frameCnt )
{
  uint8_t n, i;

  // sensor name in frame 0
  if( frameCnt == 0 )
  {                                                       // sensor name
    m_exBuffer[2] = 0x00;  			                          // 2Bit packet type(0-3) 0x40=Data, 0x00=Text 
    m_exBuffer[8] = 0x00;                                 // 8Bit Sensor Id, 0 = Sensor Name
    m_exBuffer[9] = m_nameLen<<3;                         // 5Bit description, 3Bit unit length (use one space character)
    memcpy( &m_exBuffer[10], m_name, m_nameLen );         // copy label plus unit to ex buffer starting from pos 10
    if( DevIdx )                                          // kommt noch ein Sensor Index hinten dran ?
      m_exBuffer[m_nameLen-1+10] = DevIdx;
    n = 10 + m_nameLen;
  }
  // sensor dictionary: use the first few frames with even numbers to transfer 
  else if( ( (frameCnt/2) <= m_nSensors && (frameCnt % 2) == 0 ) )
  {
    for( int nDict = 0; nDict < m_nSensors; nDict++ )
    {
      JetiSensor sensor( m_dictIdx, this );
      if( ++m_dictIdx >= m_nSensors )
        m_dictIdx = 0;

      if( sensor.m_bActive )
      {
        m_exBuffer[2] = 0x00;                                          // 2Bit packet type(0-3) 0x40=Data, 0x00=Text
        m_exBuffer[8] = sensor.m_id;  	                               // 8Bit id
        m_exBuffer[9] = (sensor.m_textLen<<3) | sensor.m_unitLen;	     // 5Bit description, 3Bit unit length 
        n = sensor.jetiCopyLabel( m_exBuffer, 10 ) + 10;               // copy label plus unit to ex buffer starting from pos 10
        break;
      }
    }
  }
  // send EX values in all other frames
  else
  {
    int bufLen;
    int nVal = 0;                                                  // count values 
    m_exBuffer[ 2 ] = 0x40;						                             // 2Bit Type(0-3) 0x40=Data, 0x00=Text
    n=8;						                                               // start at nineth byte in buffer

    do
    {
      bufLen = 0;                                                           // last value buffer length    
      JetiSensor sensor( m_sensorIdx, this );
      if( ++m_sensorIdx >= m_nSensors )                                     // wrap index when array is at the end
        m_sensorIdx = 0;

      if( sensor.m_bActive && sensor.m_value != -1 )                        // -1 is "invalid"
      {
	      if( sensor.m_id > 15 )
		    {
		      m_exBuffer[n++] = 0x0 | (sensor.m_dataType & 0x0F);               // sensor id > 15 --> put id to next byte
		      m_exBuffer[n++] = sensor.m_id;		
		    }
		    else
		      m_exBuffer[n++] = (sensor.m_id<<4) | (sensor.m_dataType & 0x0F);  // 4Bit id, 4 bit data type (i.e. int14_t)
			
        bufLen = sensor.m_bufLen;
        n += sensor.jetiEncodeValue( m_exBuffer, n );
      }
      if( ++nVal >= m_nSensors )                                            // dont send twice in a frame
        break;
    }
    while( n < ( 26 - bufLen ) );                                           // jeti spec says max 29 Bytes per buffer
  }

  // complete some more EX frame data
  m_exBuffer[0] = 0x7E;                m_exBuffer[1] = 0x2F;			          // EX-Frame Separator
  m_exBuffer[2] |= n-2;					                                            // frame length to Byte 2
  m_exBuffer[3] = MANUFACTURER_ID_LOW; m_exBuffer[4] = MANUFACTURER_ID_HI;  // sensor ID
  m_exBuffer[5] = m_devIdLow;          m_exBuffer[6] = m_devIdHi;
  m_exBuffer[7] = 0x00; // reserved (key for encryption)

  // calculate crc
  m_exBuffer[n] = jeti_crc8( m_exBuffer, n );

  // serial transmission
  m_pSerial->Send( 0x7E, false );                                 // send EX frame header tag
  for( i = 1; i <= n; i++ )                                       // followed by EX data frame (start from byte 1, since 0x7e has already been sent)
    m_pSerial->Send( m_exBuffer[i], true );
}


// **************************************
// Helpers
// **************************************
// merge name and unit and terminate with '\0'
void JetiSensor::copyLabel( const uint8_t * name, const uint8_t * unit,  uint8_t * dest, int dest_size, uint8_t * nameLen, uint8_t * unitLen )
{
  int maxchar = dest_size -1 ;

  uint8_t i = 0, j = 0;
  while( name[i] != '\0' && j < maxchar )
    dest[ j++ ] = name[ i++ ];
  *nameLen = i;

  i = 0;
  while( unit[i] != '\0' && j < maxchar )
    dest[ j++ ] = unit[ i++ ];
  *unitLen = i;

  dest[ j ] = '\0';
}

// encode sensor value to jeti ex format and copy to buffer
uint8_t JetiSensor::jetiEncodeValue( uint8_t * exbuf, uint8_t n )
{
  switch( m_dataType )
  {
  case TYPE_6b:
    exbuf[n]  = ( m_value & 0x1F) | ((m_value < 0) ? 0x80 :0x00 );                   // 5 bit value and sign 
    exbuf[n] |= m_precision;                                                         // precision in bit 5/6 (0, 20, 40)
    return 1;
	
  case TYPE_14b:
    exbuf[n]      = m_value & 0xFF;                                                  // lo byte
    exbuf[n + 1]  = ( (m_value >> 8) & 0x1F) | ((m_value < 0) ? 0x80 :0x00 );        // 5 bit hi byte and sign 
    exbuf[n + 1] |= m_precision;                                                     // precision in bit 5/6 (0, 20, 40)
    return 2;

  case TYPE_22b:
    exbuf[n]      = m_value & 0xFF;                                                  // lo byte
    exbuf[n + 1]  = (m_value >> 8 ) & 0xFF;                                          // mid byte
    exbuf[n + 2]  = ( (m_value >> 16) & 0x1F) | ((m_value < 0) ? 0x80 :0x00 );       // 5 bit hi byte and sign 
    exbuf[n + 2] |= m_precision;                                                     // precision in bit 5/6 (0, 20, 40)
    return 3;

  case TYPE_DT:
    exbuf[n]      = m_value & 0xFF;                                                  // value has been prepared by SetSensorValueDate/Time 
    exbuf[n + 1]  = (m_value >> 8 ) & 0xFF;
    exbuf[n + 2]  = ( (m_value >> 16) & 0xFF) | ((m_value < 0) ? 0x80 :0x00 );
    return 3;

  case TYPE_30b:
    exbuf[n]      = m_value & 0xFF;                                                  // lo byte
    exbuf[n + 1]  = (m_value >> 8 ) & 0xFF;
    exbuf[n + 2]  = (m_value >> 16 ) & 0xFF;
    exbuf[n + 3]  = ( (m_value >> 24) & 0x1F) | ((m_value < 0) ? 0x80 :0x00 );       // 5 bit hi byte and sign 
    exbuf[n + 3] |= m_precision;                                                     // precision in bit 5/6 (0, 20, 40)
    return 4;

  case TYPE_GPS:
    exbuf[n]      = m_value & 0xFF;                                                  // value has been prepared by SetSensorValueGPS 
    exbuf[n + 1]  = (m_value >> 8 ) & 0xFF;                                          
    exbuf[n + 2]  = (m_value >> 16) & 0xFF;
    exbuf[n + 3]  = (m_value >> 24) & 0xFF;
    return 4;
  }
  return 0;
}

// copy sensor label to ex buffer
uint8_t JetiSensor::jetiCopyLabel( uint8_t * exbuf, uint8_t n )
{
  uint8_t i = 0;

  while( m_label[i] != '\0' )
    exbuf[ n++ ] = m_label[ i++ ];

  return( i ) ; // number of bytes copied
}


// **************************************
// Jeti helpers
// **************************************

// Published in "JETI Telemetry Protocol EN V1.06"
//* Jeti EX Protocol: Calculate 8-bit CRC polynomial X^8 + X^2 + X + 1
uint8_t JetiExProtocol::update_crc (uint8_t crc, uint8_t crc_seed)
{
  unsigned char crc_u;
  unsigned char i;
  crc_u = crc;
  crc_u ^= crc_seed;
  for (i=0; i<8; i++)
    crc_u = ( crc_u & 0x80 ) ? POLY ^ ( crc_u << 1 ) : ( crc_u << 1 );
  return (crc_u);
}

//* Calculate CRC8 Checksum over EX-Frame, Original code by Jeti
uint8_t JetiExProtocol::jeti_crc8 (uint8_t *exbuf, unsigned char framelen)
{
  uint8_t crc = 0;
  uint8_t c;

  for(c=2; c<framelen; c++)
    crc = update_crc (exbuf[c], crc);
  return (crc);
}
