/*********************************************************\
*  AVR MegaSquirt - 2003                                  *
*  (C) 2003 [Michael Kristensen, mik@caffrey.dk]          *
***********************************************************
*  -- History --                                          *
*    release 0.1 - 06-03-2003                             *
*            http://caffrey.dk/megasquirt                 *
*    initial Motorola version by Bowling & Grippo (2002)  *
*            http://www.bgsoflex.com/megasquirt.html      *
\*********************************************************/


/*
TODO:
The letters of the non-standard comm. commands are a bit too random, 
fix it before it is too late!
*/


/*
  This file deserves a little explanation!

  Instead of creating a huge circular buffer for uart data, 
  a smaller circular buffer 'command_queue' are used instead. 
  This buffer contains the address and arguments to functions which 
  can fetch bytes from any place in the memory.

  eg. MegaTune sends an 'A' to request realtime variables:
  1) Uart interrupt calls comm()
  2) comm pushes the address of sendRTvar and the number of 
     bytes to send (22) onto the command queue and enables tx-interrupt.
  3) tx-interrupt reads the command queue, executes the function and
     decrements the number of bytes to send (and stores it in the 
     command queue again). If number equals zero remove the function 
     from the queue and start over. If the queue is empty, then disable 
     interrupt.
*/

   /*!
   * \addtogroup g_serial_comm Serial Communication Routines
   */
/*@{*/


#include "global.h"
#include "comm.h"
#include "ringbuf.h"
#include "storage.h"
#include "actuators.h"
#include "ve.h"
#include "iac.h"
#include "adc.h"
#include "lcd.h"
#include "ego.h"
#include "fuelcalc.h"
#include "helpers.h"
#include "benchmark.h"
#include "wbo2.h"
#include "boostcontrol.h"
// #include "sipr.h"
#include "spi.h"
#include "firmware_revision.h"
#include "knock.h"


#include <avr/io.h>
#include <inttypes.h>
#include <avr/sfr_defs.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#include "varstr.h"

extern volatile struct engine_t engine;
// extern volatile struct time_t rtc;

extern uint8_t code_ver;

#include "arithm.c"

struct cmd_queue_t cmd_queue[CMD_QUEUE_SIZE];
uint8_t cmd_queue_tail, cmd_queue_head;

uint8_t comm_state;
uint8_t comm1_relay_allowed;

char vems_string[] PROGMEM = "VEMS Genboard v3, ";

/* baudrate change pending flag. Ignored when 0,
 * otherwise it contains the new baudrate divisor */
uint8_t new_baudrate;


#if ((defined WBO2) || (defined KNOCK_8101))
uint8_t comm_TX_plainbytes[comm_TX_plain_SIZE];
#endif

const uint8_t oneWireInit[13] PROGMEM = {0xc1, 0xe1, 0x33, 0xff,  0xff, 0xff, 0xff, 0xff,  0xff, 0xff, 0xff, 0xe3,  0xc1};

# define oneWireInitLen 13

 /** send echo via UART0,
 * send 1-wire magic sequence via UART1
 * and allow bytes coming from UART1 within next 2.1 seconds to be relayed back to UART0
 */
void test_1wire()
{
	uint8_t i;
	ringbuf_push(&comm_sndbuf, '-' );
	pushfuncpar( &srl_direct, 4, (void*)( ('+' << 8) | 'u' ) ); // will print "+uzz"

	UCSR0B |= _BV(UDRIE0); // enable transmit interrupt for UART0, so comm_sndbuf does not get stuck
	// allow 2.1 sec window for bytes coming from UART1 to be transmitted (relayed) via UART0:
	comm1_relay_allowed = 21; // 21 * 0.1 seconds

	for(i=0; i< oneWireInitLen; i++){		
		while( (UCSR1A&0x20) == 0 )  ;  // UDRE1 bit means: TX-Data Register Empty   

		UDR1 = pgm_read_byte( &(oneWireInit[i] ));
	}
}

/***************************************************************************/
/*** comm                                                                ***/
/***************************************************************************/
void comm(uint8_t rx_data) {
  static uint8_t rx_offset = 0;
  static uint8_t rx_cmd = 0;

  switch (comm_state) {
  case OFFSET:
    rx_offset = rx_data;  // the offset for storing the new byte
    comm_state = VALUE;
    break;

  case VALUE:
    switch (rx_cmd) {
      case 'p':
		change_page(rx_data);	// change to page 'rx_data'
		break;
      case 'w':
		store_page(rx_offset, rx_data);
		break;
		
	  /*
      * new paged writing and reading commands, a separate command for each page write / read
	  * this fixes the config/other overwrite on ecu reset bug
	  */
	
	  case 'O':
		change_page(0);
		store_page(rx_offset, rx_data);
		break;
	  case 'H':
		change_page(1);
		store_page(rx_offset, rx_data);
		break;	
	  case 'T':
	    change_page(2);
		store_page(rx_offset, rx_data);
		break;	
      case 'J':
		change_page(4);
		store_page(rx_offset, rx_data);
		break;	
      case 'N':
		change_page(6);
		store_page(rx_offset, rx_data);
		break;	
      case 'L':
		change_page(7);
		store_page(rx_offset, rx_data);
		break;

#ifdef BOOSTCONTROL
      case 'Y': // boost
		change_page(8);
		store_page(rx_offset, rx_data);
		break;
#endif
		
	  /*
      * eof new paged writing and reading commands
	  */	
	
      default:
        break;
    }
    comm_state = CMD; // back to normal mode
    break;

  default:
    switch (rx_data) {
    case 'A': // send all realtime values
	  pushfunc(&MTsendRTvar, MT_RTVAR_COUNT);
      break;

    case 'B': // store to eeprom
      save_variables(0);
      break;

    case 'C': // test communications
      pushfunc(&MTtestComm, 1);
      break;
    case 'D': // send all realtime values
	  pushfunc(&MTsendRTvar, MT_RTVAR_COUNTMORE);
      break;
    case 'p': // request page number 'p'+<page>
      comm_state = VALUE;
      rx_cmd = 'p';
      break;

    case 'Q': // send code version
      pushfunc(&MTcodeVer, MT_CODEVER_LEN);
      break;

    case 'S': // send textual signature
      pushfunc(&sendSignature, 24);
      break;

    case 'v': // send page
	  pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page);  // don't know the length yet
      break;

    case 'w': // receive value at 'w'+<offset>+<newbyte> in page debug.megatune_page
      comm_state = OFFSET;
      rx_cmd = 'w';
      break;
	  
	/*
    * new paged writing and reading commands, a separate command for each page write / read
	* this fixes the config/other overwrite on ecu reset bug
	*/

	// config read
	case 'o':
		change_page(0);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
		
	// config write
	case 'O':
		comm_state = OFFSET;
		rx_cmd = 'O';
		break;
		
	// h[] table read
	case 'h':
		change_page(1);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
		
	// h[] table write
	case 'H':
		comm_state = OFFSET;
		rx_cmd = 'H';
		break;
		
	// tps transfer and boosttable read
	case 't':
		change_page(2);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
		
	// tps transfer and boosttable write
	case 'T':
		comm_state = OFFSET;
		rx_cmd = 'T';
		break;
		
	// ve ref table read	
	case 'j':
		change_page(4);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
	
	// ve ref table write
	case 'J':
		comm_state = OFFSET;
		rx_cmd = 'J';
		break;
		
	// advance table read	
	case 'n':
		change_page(6);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
	
	// advance table write
	case 'N':
		comm_state = OFFSET;
		rx_cmd = 'N';
		break;
		
	// lambda table read
	case 'l':
		change_page(7);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;
		
	// lambda table write	
	case 'L':
		comm_state = OFFSET;
		rx_cmd = 'L';
		break;

    // boost Ref table read
#ifdef BOOSTCONTROL
	case 'y':
		change_page(8);
		pushfuncpar(&send_page, 0xff, (void *)(uint16_t)debug.megatune_page); 
		break;

	case 'Y':
	    // boost Ref table write	
		comm_state = OFFSET;
		rx_cmd = 'Y';
		break;
#endif
		
	/*
    * eof new paged writing and reading commands
	*/	

    default:
      break;
    }
  }
}


/***************************************************************************/
/***************************************************************************/
/*** MegaTune compatibility functions                                    ***/
/***************************************************************************/
/***************************************************************************/

uint8_t sendSignature(uint8_t i) {
  if (i < 18) { // TODO: check sizeof ?
        return pgm_read_byte( (uint8_t *)&vems_string + i );
  }
  i -= 18;
  if (i < 6)
        return *( (uint8_t *)&FIRMWARE_VERSION_STR + i );
  return 0;
}

/***************************************************************************/
/*** codeVer                                                             ***/
/***************************************************************************/
// return codeversion "VEMS v1.1 12x12 kpa=2,1"
uint8_t MTcodeVer(uint8_t i) {
  switch(i){
  	case 0: return 'V';
  	case 1: return 'E';
  	case 2: return 'M';
  	case 3: return 'S';
  	case 4: return ' ';
  	case 5: return 'v';
  	case 6: return '0' + FIRMWARE_VERSION_MAJOR;
  	case 7: return '.';
  	case 8: return '0' + FIRMWARE_VERSION_MINOR;
  	case 9: return ' ';
  	case 10: return '0' + VE_SIZE_RPM / 10; // TODO: make sure gcc optimizes this constant
  	case 11: return '0' + VE_SIZE_RPM % 10;
  	case 12: return 'x';
  	case 13: return '0' + VE_SIZE_KPA / 10; // TODO: make sure gcc optimizes this constant
  	case 14: return '0' + VE_SIZE_KPA % 10;
  	case 15: return ' ';
  	case 16: return 'k';
  	case 17: return 'p';
  	case 18: return 'a';
  	case 19: return '=';
  	case 20: return KPAFAC_UNIT == 0xF ? '2' : hex_hi(config.kpa_conf);
  	case 21: return ',';
  	case 22: return KPA_CONFIG_RESOLUTION == 0xF ? '1' : hex_lo(config.kpa_conf);
	default: return 'V';
    }
#if(MT_CODEVER_LEN != 23)
#warning "MT_CODEVER_LEN size mismatch"
#endif

}

/***************************************************************************/
/*** testComm                                                            ***/
/***************************************************************************/
// return low part of second-count
uint8_t MTtestComm(uint8_t i) {
  return (rtc.sec & 0xFF);
}



/***************************************************************************/
/*** getMCPdataHIGH                                                      ***/
/***************************************************************************/
// does not start a adc conversion, just masks out the high byte of latest conversion 
// and returns if the channel data is not 0xffff

// channels range from 0-7 internally (from 1 to 8 according to datasheet)

uint8_t getMCPdataHIGH(uint8_t i) {

	uint16_t t;
	uint8_t r = 0;

#ifdef GENBOARDv3
	if(i < 8) {
		t = mcp3208_data[i];
		if (t != 0xffff) 
	//		r = (uint8_t) t >> 8;
			r = t >> 8;
	}
#endif
	
	return r;	
}


/***************************************************************************/
/*** getMCPdataLOW                                                       ***/
/***************************************************************************/
// starts an mpc channel adc conversion on the appropriate channel
// and if conversion is finished return low byte of adc result, else 0

// channels range from 0-7 internally (from 1 to 8 according to datasheet)

uint8_t getMCPdataLOW(uint8_t i) {

	uint16_t t;
	uint8_t r = 0;

#ifdef GENBOARDv3
	if(i < 8) {
		t = mcp3208_data[i];
		if (t != 0xffff) 
			r = (uint8_t) t & 0xFF;
		read_mcp3208(i);
	}
#endif
	
	return r;	
}




/***************************************************************************/
/*** sendRTvar                                                           ***/
/***************************************************************************/
// convert datastructures to megasquirt compatible format

uint8_t MTsendRTvar(uint8_t i) {
//	uint8_t a, b;

  switch (i) {
  case 0:
    return (uint8_t)rtc.sec;  //secl

  case 1:
/* TODO:
  check this.
  "squirt" is apparently a bitmap of the something:
  ....BBAAba
  where
  capital A, B : ????
  lowercase a, b: if the given channels are actually squirting
  // each squirt_t has its own status field, merge these fields
    a = inj_port1.status;
    b = inj_port2.status;

    return ((a & 0x01) 
    		| ((a & 0x06) << 1)
           	| ((b & 0x01) << 1) 
           	| ((b & 0x06) << 3));
*/

  case 2:  
// remove the 'idle' bit to avoid confusing mstweak: return (engine.status & (uint8_t)~_BV(idle));  //engine
	  return engine.status;  //engine
  case 3:  return sensors8(BARO);  //baro

  // high resolution MAP or load for AlphaN
  case 4: return (config.hybrid_rpm_m ? engine.kpa_tps_blend << 2 : engine.kpa250) >> 8;
  case 5: return (config.hybrid_rpm_m ? engine.kpa_tps_blend << 2 : engine.kpa250) & 0xFF;

  case 6: return engine.mat;   //mat
  case 7: return engine.coolant;   //clt
  case 8: return engine.tps;   //tps
  case 9: return engine.batt;  //batt
  case 10: // o2
#ifdef WBO2
	if (WBO2_SENSOR_CONFIGURED)
		return wbo2.Ip_output;
	else
#endif
		return ego_voltage();  //ego 
  case 11: return 50+((corr.ego*100)>>8);	//egocorr
  case 12: return corr.air;       //aircorr
  case 13: return corr.warm >= 155 ? 255 : corr.warm+100;      //warmcorr

  // high resolution RPM
  case 14: return engine.rpm_vhr >> 8;
  case 15: return engine.rpm_vhr & 0xFF;

  case 16: return inj_port[0].pwcalc >> 8;     //pw high
  case 17: return inj_port[0].pwcalc & 0xFF;   //pw low
  case 18: return corr.baro;      //barocorr
  case 19: {	// calculate from the stored high-res value (/100 and railing is needed)
		uint16_t ge;
		ge = mult16_8(corr.gammae_1k5, 17) >> 8;	// 1% error in reporting...

		if ((ge >> 8) != 0)  // check for overflow
			return 0xFF;
		else
			return (uint8_t)ge;
	}
  case 20: return corr.ve_hp>>8;

// should we use this style ? 
// #define MT_RT_OFFSET_XXX (21)
// #define MT_RT_OFFSET_IGNADV (MT_RT_OFFSET_XXX+1)

  case 21: return engine.dwell>>4;	// 64 usec resolution (possibly but unlikely overflowing)
  case 22: return (engine.ignadv < 510 ? (engine.ignadv>>1) : 255); // 128 + ignadv_halfdeg

  case 23: return iac.position;

// make sure 16 bit variables start on even offsets:

// raw egt data, do scaling externally (in megatune based on egtofs and egtcal)
  case 24: return getMCPdataHIGH(4); // EGT1 HIGH
  case 25: return getMCPdataLOW(4);  // EGT1 LOW
  case 26: return getMCPdataHIGH(3); // EGT2 HIGH
  case 27: return getMCPdataLOW(3);  // EGT2 LOW
  
// also raw adc data for te other sensors 
  case 28: return getMCPdataHIGH(6); // EBP HIGH
  case 29: return getMCPdataLOW(6);  // EBP LOW
  case 30: return getMCPdataHIGH(7); // FP HIGH
  case 31: return getMCPdataLOW(7);  // FP LOW
  case 32: return getMCPdataHIGH(5); // MISC1 HIGH
  case 33: return getMCPdataLOW(5);  // MISC1 LOW
  case 34: return getMCPdataHIGH(0); // MISC2 HIGH midopa
  case 35: return getMCPdataLOW(0);  // MISC2 LOW
  case 36: return getMCPdataHIGH(1); // MISC3 HIGH midopa
  case 37: return getMCPdataLOW(1);  // MISC3 LOW
  case 38: return getMCPdataHIGH(2); // MISC4 HIGH
  case 39: return getMCPdataLOW(2);  // MISC4 LOW
  
  case 40: {
		uint16_t mt_val;
		mt_val = mult16_8((corr.lambda + LAMBDA_INT_OFFSET), 72) >> 7;
		mt_val = (mult16_8(div_65535_x(mt_val), 72) >> 7) - MT_LAMBDA_OFFSET;
 		return mt_val & 0xFF;
	}
	
  // dump knock related variables for cylinder 0	
#ifdef GENBOARDv3
  case 41: return knock_cyl[0].knock_val >> 8;
  case 42: return knock_cyl[0].knock_val & 0xFF;
  case 43: return knock_cyl[0].noise_val >> 8;
  case 44: return knock_cyl[0].noise_val & 0xFF; 
  case 45: return knock_cyl[0].knock_adjust;
  case 46: return knock_cyl[0].state;	
	
#else //ver2.2
  case 41: return 0;
  case 42: return 0;
  case 43: return 0;
  case 44: return 0; 
  case 45: return 0;
  case 46: return 0;	
#endif	
  // status bytes like WBO2 activated and alike
  case 47: {
  
	uint8_t status = 0;
	uint8_t channel = config.shiftcut_channel;
	uint8_t shiftcut_conf = config.shiftcut_conf;
#ifdef GENBOARDv3
	uint16_t t = mcp3208_data[channel];
	
#else
	uint16_t t=0;
#endif	
	#define SHIFTCUT_THRESHOLD 0xFF
		
#ifdef ALS
	if (!als_deactivated()) status |= 0x80;	// als switch active			
	if (is_als_active()) status |= 0x01; // als function active
#endif
#ifdef SOFTLAUNCH
	if (!launch_deactivated()) status |= 0x40; // launch control switch active
#endif
	if (t < SHIFTCUT_THRESHOLD && channel < 8 && shiftcut_conf > 1) status |= 0x20; // shift cut
	if (engine.sparkcutlvl) status |= 0x10; // igncut active
	if (engine.status & _BV(idle)) status |= 0x08;	// idle control
	if (engine.status_ext & _BV(o2_closed_loop)) status |= 0x04; // closed loop (ego)
	if (benchstats.wheel_err > 10) status |= 0x02; // significant wheel errrors
	
	return status;
	
  }
#ifdef BOOSTCONTROL
// without boostcontrol the default 0 will be returned
  case 48: return boost_target() >> 5; // functions wastes clocks for SRAM
  case 49: return engine.boost_command_duty; // boost solenoid duty
  case 50: return (uint8_t)((boostcontrol_pid.integral+0x4000)>>7); // boost PID integral, assume PID_INTEGRAL_EXP=6
#endif
  case 51: return (uint8_t)((iac.integral+0x8000)>>8); // iac.integral, assume INTEGRAL_LIMIT_SCALE=7
  case 52: engine.status_ext &= ~_BV(individual_allowed); return engine.sparkcutlvl; // 0 .. 16 ( 0 is no igncut, normal operation)
case 53: return individual_power[0];
case 54: return individual_power[1];
case 55: return individual_power[2];
case 56: return individual_power[3];
case 57: return individual_power[4];
case 58: return individual_power[5];
case 59: return individual_power[6];
case 60: engine.status_ext |= _BV(individual_allowed); 
	if (debug.enable_flags & _BV(7)){
		pushfunc(&MTsendRTvar, MT_RTVAR_COUNT);
	}
	return individual_power[7];
case 61: return corr.tpsaccel;  //tpsaccel

  default: return 0;
  }
}

/** send 32 bytes:
 * first 16 bytes is the 8 mcp3208 in big endian
 * 2nd 16 bytes is the 7 internal AD in big endian, but channel 7 first (0 latest)
 */
uint8_t allSensorRaw(uint8_t i)
{
	uint8_t t8, *p;
	if( i < 16 ){ // 0 .. 15
		t8 = i >> 1;
		if( i & 1 ){ // LOW sent AFTER high byte
			return getMCPdataLOW( t8 );
		} else { // send high FIRST:
			return getMCPdataHIGH( t8 );
		}
	}
	p = (uint8_t*)sensor_filt; // convert to byte array
	return p[ 31 - i ];
}

/* Dump the table 
* private higher byte holds the table identifier char,
* private lower byte holds the table-line
* line higher 4 bits contain the counter (eg 0..7)
* line lower 4 bits contain the maxvalue of counter (eg 7)
* count (conservatively) holds the character inside the line
*/
uint8_t TableDump(uint8_t cnt){
	uint16_t t;
	t = (uint16_t)(cmd_queue[cmd_queue_head].private);
	char table; 
	uint8_t line, linemax, *p, len;
	uint8_t block_printlen, block_bytes, line_end;

	table = t>>8;
	line = t & 0xFF;
	linemax = line & 0x0F;
	line >>= 4;
#define HEADER_LENGTH (5)
	if(cnt < HEADER_LENGTH){ // line beginning
		switch(cnt){
			case 0: return table;
			case 1: return '[';
			case 2: return hex_lo(line);
			case 3: return ']';
			case 4: return '=';
		}	// switch
	}	// if
		
	if(cnt >= 0xF0){
		// similar to state 0, but 0 cannot be used again for some reason...
		cmd_queue[cmd_queue_head].count = 0;
		return table;	
	}
	
	block_printlen = 3;
	block_bytes = 1;

	switch(table){ // this should get from a place common with menu
		case 'j':
#ifdef VE_TABLE_FIX
			// 16 bit table:
			block_printlen = 5;
			block_bytes = 2;
			p=((uint8_t*)ve.ve_table) + 1;
#else
			p=ve.ref_table;
#endif
			len = VE_SIZE_RPM;
			break;
		case 'k':
			p=ve.kpa_range;
			len = VE_SIZE_KPA;
			break;
		case 'l':
			p=ve.lambda;
			len = VE_SIZE_RPM;
			break;
		// either ifndef NOIGN is not needed, or needed in menu.c too
		// I allow the read-write of 'n' table for other applications...
		case 'n':
			p=ign.adv_table;
			len = VE_SIZE_RPM;
			break;
#ifdef HWMAP
		case 'h':
			p=hwmap.table;
			len = HWMAP_SIZE;
			break;
#endif
#ifdef BOOSTCONTROL
		case 'b':
			p = boost.boostmaxtarget_table;
			len = VE_SIZE_RPM;
			break;
		case 't':
			p = boost.tpstransfer_table;
			len = TPSTRANSFERTABLE_SIZE;
			break;
		case 'y':
			p = boost.boost_ref_table;
			len = VE_SIZE_RPM;
			break;
#endif
		case 'r':
		default:
			p=ve.rpm_range;
			len = VE_SIZE_RPM;
		// TODO: dump learnt values and difference of learnt and ve_ref
	}

	// we must determine how long the line is for the given table:
	// 3 * len or 5 *len for a 16 bit table 
	// (-1 in any case, since no space before first, see the '\r' branch)
	line_end = HEADER_LENGTH + len * block_printlen;	
	if(cnt == line_end){ // shell we finish or go to next line
		if(line != linemax){ // not finished
			line++;	// could be made faster, but ....
			cmd_queue[cmd_queue_head].private = (void*)((table<<8) + (line<<4) + linemax);
			cmd_queue[cmd_queue_head].count = 0xF0-1; // will be incremented (0-1 doesnt work)
		} else { // FF would be better?
			cmd_queue[cmd_queue_head].count = 0xFE; // set to len => THE END
		}
		return '\n';
	}
	
	if(cnt == line_end - 1){
		return '\r';
	}
	
	cnt -= (HEADER_LENGTH - 1);

	p += len * line * block_bytes;

	while(cnt >= block_printlen){
		cnt -= block_printlen;
		p += block_bytes;
	}

	switch(cnt){	// every byte is printed to 3 characters
		case 0: return ' ';
		case 1: return hex_hi(*p);
		case 2: return hex_lo(*p);
		case 3: return hex_hi(*(p-1));	// only for 16 bit table (little endian)
		case 4: return hex_lo(*(p-1));			
		default: return ' ';
	}
}

/* Dump the config structure:
 * We keep track of line number in private.
 * position inside the line is in count, but somewhat tricky 
 * Note that len is 0xFE because of pushfunc() hack, although we specified 0xFF
 */
uint8_t ConfigDump(uint8_t cnt) {
	uint8_t c, i, *str; 
	uint16_t idx;
	PGM_VOID_P P;
	// not always needed, but almost:
	// if(cnt == 0xF7) cmd_queue[cmd_queue_head].count = cnt = 0; // needed because of the len-1 hack
	
	idx = (uint16_t)(cmd_queue[cmd_queue_head].private);
	P = (PGM_VOID_P)(varstr + idx);
	i = 0;
	while((str=(void*)PRG_RDW(P)) == NULL){
		P -= 2;  // addresses are two-byte
		i++;
	}
#define LAST_ELEMENT (sizeof(struct config_t)-1)
	if(cnt < 0xF8){
		c = PRG_RDB(str + cnt);
		if(c) {
			return c;
		} else { // end of string
		// we need the [.]= if i>0 or the next pointer is NULL but not at the very end
			if(i || (((void*)PRG_RDW(P+2)) == NULL && idx < LAST_ELEMENT)){ // no need for [i]=.. just =..
				cmd_queue[cmd_queue_head].count = 0xF8 - 1 ; // will be incremented!
				return '[';
			} else {
				cmd_queue[cmd_queue_head].count = 0xFB - 1 ; // will be incremented!
				return '=';
			}
		}	
	}
	switch (cnt){
		case 0xF8: return hex_lo(i);
		case 0xF9: return ']';
		case 0xFA: return '=';
		case 0xFB: return hex_hi(*((uint8_t *)(&config) + idx));
		case 0xFC: return hex_lo(*((uint8_t *)(&config) + idx));
		case 0xFD: return '\r';
		case 0xFE: // let's see: are we at the very end ?
			if((uint16_t)(++ cmd_queue[cmd_queue_head].private) <= LAST_ELEMENT ){
				// cmd_queue[cmd_queue_head].count = 0xFE;	// end of the show
				// = cmd_queue[cmd_queue_head].len 
				cmd_queue[cmd_queue_head].count = 0xFF; // show must go on next line
			};
			return '\n';
	}
	return 'z';
}
/***************************************************************************/
/***************************************************************************/
/*** End of MegaTune compatibility functions                             ***/
/***************************************************************************/
/***************************************************************************/




/***************************************************************************/
/*** sendConfigVar                                                       ***/
/***************************************************************************/
uint8_t sendConfigVar(uint8_t i) {
  if (i < sizeof(struct config_t))
    return *( (uint8_t *)&config + i );
  else 
    return 0;
}


/*
 * returns upper and lower byte of "private"
 */
uint8_t srl_direct( uint8_t i ){
	uint16_t t = (uint16_t) cmd_queue[cmd_queue_tail].private;
	switch(i){
		case 0: {
			return t>>8;
		}
		case 1: {
			return t & 0xff;
		}
		default: return 'z';
	}
}

/************************************
* generic serial logging function.
* uses private from the actual command as a pointer (array base). 
* Unfortunately we need to know the string-length in advance 
* For constant(-length) strings this is OK.
* We could set a (global or static variable is simplest) flag
* to notify the caller about the end of transmission.
* This way other comm functions could be left untouched.
************************************/
uint8_t srl_generic(uint8_t addr)
{
	return (cmd_queue[cmd_queue_head].private)[addr]; 
}


/* same as above, but private points to PROGMEM (flash), eg. PSTR("...") */
uint8_t srl_generic_P(uint8_t addr)
{
	return PRG_RDB(&(cmd_queue[cmd_queue_head].private[addr])); 
}

// helper funtion used to access flash above the 16 bit address limit
// TODO: check that it works
uint8_t srl_generic_P_upper(uint8_t addr)
{
	uint32_t v;
	v = 0x10000 | (uint16_t)&cmd_queue[cmd_queue_head].private[addr];
	return pgm_read_byte_far(v);
}

void change_page(uint8_t page) {
	if (page > 8) { // TODO: make the 8 automatic
		page = MT_PAGE_IDX_DUMMY;
	}
	debug.megatune_page = page;
}

/* ===============================================
 * make sure MT_PAGE_IDX_... and pages_size match.
 * avoid changing these since they break several tuningsoftwares
 */

#define MT_PAGE_PTR_DUMMY (uint8_t *)&config

PROGMEM uint8_t *pages_ptr[] = {
 	(uint8_t *)&config

#ifdef HWMAP
 	,(uint8_t *)&hwmap.table
#else	//  sizeof(hwmap) bytes _must_ be transmitted to megatune
 	, MT_PAGE_PTR_DUMMY
#endif

#ifdef BOOSTCONTROL
	,(uint8_t *)&boost.tpstransfer_table
#else // no BOOSTCONTROL
 	, MT_PAGE_PTR_DUMMY
#endif
 	, MT_PAGE_PTR_DUMMY
 	,(uint8_t *)&ve.ref_table
 	,(uint8_t *)&ve.lambda
 	,(uint8_t *)&ign.adv_table
	,(uint8_t *)&ve.lambda
#ifdef BOOSTCONTROL
	,(uint8_t *)&boost.boost_ref_table
#endif
};

PROGMEM uint8_t pages_size[] = {
 	sizeof(struct config_t)
 	, HWMAP_BYTES

#ifdef BOOSTCONTROL
	,sizeof(boost.tpstransfer_table) + sizeof(boost.boostmaxtarget_table)
#else 
	, MT_PAGE_SIZE_DUMMY
#endif

	, MT_PAGE_SIZE_DUMMY
 	,sizeof(ve.ref_table)  // ve.ve_table will be sent but this many bytes :-)
 	,sizeof(ve.lambda)
 	,sizeof(ign.adv_table)
 	,sizeof(ve.lambda)
#ifdef BOOSTCONTROL
 	,sizeof(boost.boost_ref_table)
#endif
};

#define MEGATUNE_PAGE_SIZE(page) (pgm_read_byte(pages_size + page))
#define MEGATUNE_PAGE_POINTER(page) ((uint8_t*)pgm_read_word(pages_ptr + page))

/* only makes sense for commands that send data to megatune
 * (read: not for those that store)
 */
static inline void terminate_transfer( void ) {
	cmd_queue[cmd_queue_head].len = 0;
}

/* 
 * send config and tables to megatune.
 * For page 4 the (learnt) VE table is sent,
 * not the ve.ref_table reference table
 */
 
uint8_t send_page(uint8_t i) {
	uint8_t val, size, page;

	// a little clumsy, but avoids gcc warning
	page = (uint8_t)((uint16_t)cmd_queue[cmd_queue_head].private);
	size = MEGATUNE_PAGE_SIZE(page);

	if( i < size ){
		if( page == MT_PAGE_IDX_VE ) {
			// send upper byte of (learnt) ve.ve_table
			return ve.ve_table[i] >> 8;
		} 

		uint8_t *tbl = MEGATUNE_PAGE_POINTER(page) ;
		val = tbl[i];

		if( page == MT_PAGE_IDX_LAMBDAVIEW ) {
			uint16_t mt_val;
			mt_val = mult16_8((val + LAMBDA_INT_OFFSET), 72) >> 7;
			mt_val = (mult16_8(div_65535_x(mt_val), 72) >> 7) - MT_LAMBDA_OFFSET;
 			return mt_val & 0xFF;
		} else {

			if(page == MT_PAGE_IDX_BOOST) { 
				// after t[] (with size TPSTRANSFERTABLE_SIZE) there comes
				// b[] (with size BOOSTCONTROL_LINES * VE_SIZE_RPM):
				if( i >= TPSTRANSFERTABLE_SIZE ) {
					val = boost.boostmaxtarget_table[ i - TPSTRANSFERTABLE_SIZE ];
				}
			}

#ifdef BOOSTCONTROL
			if (( page < MT_PAGE_IDX_VE || page > MT_PAGE_IDX_BOOST_REF) && ( i >= size-1 )) {
#else
			if (( page < MT_PAGE_IDX_VE || page > MT_PAGE_IDX_LAMBDAVIEW) && ( i >= size-1 )) {
#endif
		// for the 4..7 pages we pad rpm_range and kpa_range arrays to the end
		// but all other pages end normally:
				terminate_transfer();
			}
		}

		return val;
	}
	
	// note that only pages 4..6 can get to this point
	val = 0; // suppress gcc warning
	// pad rpm_range and kpa_range arrays to the end	
	i -= size;
	if (i < VE_SIZE_RPM) {
		return ve.rpm_range[i];
	}

	i -= VE_SIZE_RPM;

	if (i < VE_SIZE_KPA ){
		val = ve.kpa_range[i];
	}
	
	if (i >= (VE_SIZE_KPA - 1)) {
		terminate_transfer();
	}

	return val;
}

/* caller is responsible for idx < VE_SIZE_KPA */
void kpa_range_store( uint8_t idx, uint8_t value ){
	ve.kpa_range[idx] = value;
}

/* caller is responsible for idx < VE_SIZE_RPM */
void rpm_range_store( uint8_t idx, uint8_t value ){
	ve.rpm_range[idx] = value;
}

/* for VE_TABLE_PAGE the 
 * learnt ve.ve_table is also written, 
 * besides ve.ref_table
 */
void store_page(uint8_t addr, uint8_t value) {
	uint8_t size, page;

	page = debug.megatune_page;
	size = MEGATUNE_PAGE_SIZE(page);

	if (addr < size) {
		uint8_t *tbl;

		if( page == MT_PAGE_IDX_VE ) {
			uint16_t t16 = (value<<8) | 0x80;
			// writing to hi-res ve.ve_table
			ve.ve_table[addr] = t16;
		} else if(page == MT_PAGE_IDX_BOOST) {
			// after t[] there comes b[]:
			if( addr >= TPSTRANSFERTABLE_SIZE ){
				boost.boostmaxtarget_table[ addr - TPSTRANSFERTABLE_SIZE ] = value;
				return;
			}
		} else if(page == MT_PAGE_IDX_LAMBDAVIEW) { 
			uint16_t int_val;
			int_val = mult16_8((value + MT_LAMBDA_OFFSET), 72) >> 7; 
			int_val = mult16_8(div_65535_x(int_val) , 72) >> 7;
 			value = (int_val - LAMBDA_INT_OFFSET) & 0xFF;
 		}
 	
		// the ve.ref_table is written besides the hi-res ve.ve_table above:
		tbl = MEGATUNE_PAGE_POINTER(page);
		tbl[addr] = value;
		return;
	}

	/* 
	 * rpm_range and kpa_range arrays live at the end
	 */
#ifdef BOOSTCONTROL
	if ( page < MT_PAGE_IDX_VE || page > MT_PAGE_IDX_BOOST_REF ) {
#else
	if ( page < MT_PAGE_IDX_VE || page > MT_PAGE_IDX_LAMBDAVIEW ) {
#endif
		// terminate_transfer() is meaningless (actually harmful) here !
		return;
	} 

	addr -= size;
	if ( addr < VE_SIZE_RPM ) {
		rpm_range_store( addr, value );
		return;
	}

	addr -= VE_SIZE_RPM;	

	if (addr < VE_SIZE_KPA ) {
		kpa_range_store( addr, value );
	}
}

/***************************************************************************/
/*** pushfunc                                                            ***/
/***************************************************************************/
uint8_t pushfunc(uint8_t (*f)(uint8_t i), uint8_t len) {
  uint8_t next_tail;

  next_tail = (cmd_queue_tail + 1) % CMD_QUEUE_SIZE;
  if (next_tail == cmd_queue_head)
    return -1; // whoops, queue is full

  /* len starts at 0 */
//	if (len > 0) // this looks like a hack - it is! IT WAS!
//		len = len - 1;

  cmd_queue[cmd_queue_tail].func = f;
  cmd_queue[cmd_queue_tail].len = len;
  cmd_queue[cmd_queue_tail].count = 0;
  cmd_queue[cmd_queue_tail].private = 0;

  cmd_queue_tail = next_tail;
  
//  UCSR0B |= _BV(UDRIE0); // enable transmit interrupt
  return 0;
}

/**********************************************
* same as above, but parameter can be specified 
*/
uint8_t pushfuncpar2(uint8_t (*f)(uint8_t i), uint8_t len, const uint8_t * par, uint8_t p2) {
  uint8_t next_tail;

  next_tail = (cmd_queue_tail + 1) % CMD_QUEUE_SIZE;
  if (next_tail == cmd_queue_head)
    return -1; // whoops, queue is full

  cmd_queue[cmd_queue_tail].func = f;
  cmd_queue[cmd_queue_tail].len = len;
  cmd_queue[cmd_queue_tail].count = 0;
  cmd_queue[cmd_queue_tail].private = (void*)par;
  cmd_queue[cmd_queue_tail].privch.a = p2;
  cmd_queue_tail = next_tail;
  
//  UCSR0B |= _BV(UDRIE0); // enable transmit interrupt
  return 0;
}

// compatibility function with less parameters
uint8_t pushfuncpar(uint8_t (*f)(uint8_t i), uint8_t len, const uint8_t * par) {
	return pushfuncpar2(f, len, par, 0);
}



/***************************************************************************/
/*** initUART                                                            ***/
/***************************************************************************/
void initUART(void) {
  /* reset the baudrate flag. 0-d by gcc at startup, but ... */
  new_baudrate=0; 

  comm_state = CMD;
	  
  /* rx enable, tx enable */
  //  UCSR0B = _BV(RXCIE0) | _BV(UDRIE0) | _BV(RXEN0) | _BV(TXEN0);
  UCSR0B = _BV(RXCIE0) | _BV(RXEN0) | _BV(TXEN0);
  /* 8 databit, 1 stopbit */
  UCSR0C = _BV(UCSZ01) | _BV(UCSZ00);

  /* 9600 baud @16MHz */
  UBRR0H = 0;
  UBRR0L = 103;

  cmd_queue_tail = cmd_queue_head = 0;
}

/**
* initUART1 (1-wire)
*/
void initUART1(void)
{
  // rx enable
  //  UCSR0B = _BV(RXCIE0) | _BV(UDRIE0) | _BV(RXEN0) | _BV(TXEN0);
  UCSR1B = _BV(RXCIE1) | _BV(RXEN1) | _BV(TXEN1); // tx enable is needed
  /* 8 databit, 1 stopbit */
  UCSR1C = _BV(UCSZ11) | _BV(UCSZ10);

  /* 9600 baud @16MHz */
  UBRR1H = 0;
  UBRR1L = 103;
}

/***************************************************************************/
/*** INTERRUPT: UART rx complete ***/
/***************************************************************************/
// a small receivebuffer is still waiting to be implemented
SIGNAL(SIG_UART0_RECV) {

  // check status register for frame or overrun errors
  if ( UCSR0A & (_BV(FE0) | _BV(DOR0)) ) { 
    uint8_t dummy;
    dummy = UDR0; // just read uart and throw away the result
    return;
  } else {
	ringbuf_push(&comm_recbuf, UDR0);
    // comm(UDR0);	// TODO: put to a buffer, which menu.c consumes and calls back comm() if necessary
  }
}

/**
* INTERRUPT: UART1 (1-wire) rx complete
*/
SIGNAL(SIG_UART1_RECV) {
	uint8_t readchar;
	readchar = UDR1;	// read uart
	// check status register for frame or overrun errors
	if ( UCSR1A & (_BV(FE1) | _BV(DOR1)) ) {
		 // just and throw away the result
    		return;
	} else if( comm1_relay_allowed ){
	// inject into char-buf of UART0 directly
	// BEWARE: possibly into the middle of some bigger block, so only useful for testing
		ringbuf_push(&comm_sndbuf, readchar);
		UCSR0B |= _BV(UDRIE0); // enable transmit interrupt for UART0, so comm_sndbuf does not get stuck
  	}
}

/***************************************************************************/
/*** INTERRUPT: UART tx empty ***/
/***************************************************************************/
SIGNAL(SIG_UART0_DATA) {
	uint16_t d;

	d = ringbuf_shift(&comm_sndbuf);

	if (d>>8) { // buffer empty
                // this is tricky. To save on useless processing of the SIG_UART0_TRANS
                // interrupt we will enable that interrupt to signal us when this last byte
                // has been sent if and only if there is a baudrate change pending
		UCSR0B &= ~_BV(UDRIE0);    // disable TX interrupts
                if (new_baudrate!=0) 
		  UCSR0B |= _BV(TXCIE0);    // enable TX complete interrupt
	} else {
		UDR0 = d;
	}

}


/***************************************************************************/
/*** INTERRUPT: UART tx complete ***/
/***************************************************************************/
SIGNAL(SIG_UART0_TRANS) {
  // we can't change the baudrate when we get a TX empty interrupt because 
  // the shift register still contains the last byte to be sent. So here we
  // check to make sure the TX interrupts are disabled (nothing buffered)
  // and if the new_baudrate flag is set then we're good to change the baudrate
  // furthermore since we don't want to waste any CPU cycles we will disable
  // our interrupt.

  // if the TX interrupt is enabled then someone must have sent data while
  // we were waiting for the last byte to send and refilled the buffer so
  // disable our interrupt and bail
  if (UCSR0B & _BV(UDRIE0)) {
    UCSR0B &= ~_BV(TXCIE0);    // disable the TX complete interrupt
  }
  else {
    // if there is a baudrate change pending then make sure the cmd_queue is empty
    // and change the baudrate. Otherwise disable TX interrupts
    if ( (new_baudrate!=0) && (cmd_queue_tail==cmd_queue_head)) {
      change_baudrate(new_baudrate);
      new_baudrate=0;
      UCSR0B &= ~_BV(TXCIE0);    // disable the TX complete interrupt
    }
  }
}

// Use for debugging
void put_char(uint8_t byte)
{
    while( !(UCSR0A & 0x20) )
        ;
    UDR0 = byte;
}


