#include <LiquidCrystal.h>
#include <TimerOne.h>

#define MAX_INPUTS 8

// --------------- ADC related stuffs.... --------------------

struct input_cal_t // Struct type for input calibration values
{
	int min[MAX_INPUTS];
	int max[MAX_INPUTS];
	int center[MAX_INPUTS];
} ;
input_cal_t input_cal;

struct model_t
{
	int channels; // How many channels should PPM generate for this model ...
	float stick[8]; // The (potentially recalc'ed) value of stick/input channel.
	boolean rev[8];
	int dr[8];        // The Dual-rate array uses magic numbers :P
	/*	dr[0] = Input channel #1 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
		dr[1] = Input channel #2 of 2 for D/R switch #1. 0 means off, 1-4 valid values.
		dr[2] = Input channel #1 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
		dr[3] = Input channel #2 of 2 for D/R switch #2. 0 means off, 1-4 valid values.
		dr[4] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
		dr[5] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5.
		dr[6] = D/R value for switch # 1 LOW(off). Value -100 to 100 in steps of 5.
		dr[7] = D/R value for switch # 1 HIGH(on). Value -100 to 100 in steps of 5. 
	*/
};
volatile model_t model;

// ----------------- Display related stuffs --------------------
LiquidCrystal lcd( 12, 11, 10,     6,  7,  8,  9);
// Parameters are: rs, rw, enable, d4, d5, d6, d7 pin numbers.

// ----------------- PPM related stuffs ------------------------
// The PPM generation is handled by Timer0 interrupts, and needs
// all modifiable variables to be global and volatile...

//int max_channels = 6;  // How many channels should PPM generate ...
// Moved to model_t struct...

volatile long sum = 0;			// Frame-time spent so far
volatile int cchannel = 0; 		// Current channnel
volatile bool do_channel = true;        // Is next operation a channel or a separator


// All time values in usecs
// TODO:
// The timing here (and/or in the ISR) needs to be tweaked to provide valid
// RC PPM signals accepted by standard RC RX'es and the Microcopter...

#define framelength  21500		// Max length of frame
#define seplength      400		// Lenght of a channel separator
#define chmax         1700		// Max lenght of channel pulse
#define chmin          600		// Min length of channel
#define chwidht  (chmax - chmin)// Useable time of channel pulse

// ----------------- Menu/IU related stuffs --------------------

// Keys/buttons/switches for UI use, including dual-rate/expo
// are digital inputs connected to a 4051 multiplexer, giving
// 8 inputs on a single input pin.
#define KEY_UP    0
#define KEY_DOWN  1
#define KEY_RIGHT 2
#define KEY_LEFT  3
#define KEY_INC   4
#define KEY_DEC   5
#define KEY_DR1   6
#define KEY_DR2   7

// Voltage sense pin is connected to a 1/3'd voltage divider.
#define BATTERY_CONV (10 * 3 * (5.0f/1024.0f))
#define BATTERY_LOW 92

enum {
  VALUES,
  BATTERY,
  TIMER,
  MENU
} 
displaystate;

enum {
  TOP,
  INVERTS,
  DUALRATES,
  EXPOS, // Some radios have "drawn curves", i.e. loopup tables stored in external EEPROM ...
  DEBUG,
  SAVE
} 
menu_mainstate;
int menu_substate;

boolean keys[8];
boolean prev_keys[8];

int battery_val;

// The display/UI is handled only when more
// than UI_INTERVAL milliecs has passed since last...
#define UI_INTERVAL 250
unsigned long last = 0;

struct clock_timer_t 
{
	unsigned long start;
	unsigned long init;
	unsigned long value;
	boolean running;
} clock_timer;

// -----------------  DEBUG-STUFF --------------------
unsigned long prev_loop_time;
unsigned long avg_loop_time;
unsigned long t;


// ---------- CODE! -----------------------------------

// ---------- Arduino SETUP code ----------------------
void setup(){
  pinMode(13, OUTPUT);   // led

  pinMode(2, OUTPUT);    // s0
  pinMode(3, OUTPUT);    // s1
  pinMode(4, OUTPUT);    // s2
  pinMode(5, OUTPUT);    // e

  lcd.begin(16,2);
  lcd.print("Starting....");

  Serial.begin(9600);
  Serial.println("Starting....");
  delay(500);
  read_settings();
  scan_keys();
  if ( keys[KEY_UP])
    calibrate();

  pinMode(A5, OUTPUT);  // PPM output pin  
  do_channel = false;
  set_timer( seplength );
  Timer1.initialize(framelength);
  Timer1.attachInterrupt(ISR_timer);  

  displaystate = VALUES;
  
  // Arduino believes all pins on Port C are Analog.
  // In reality they are tri-purpose; ADC, Digital, Digital Interrupts
  // Unfortunately the interrupt mode is unusable in this scenario, but digital I/O works :P
  pinMode(A2, INPUT); 
  
  // Debugging: how long does the main loop take on avg...  
  t = micros();
  avg_loop_time = t;
  prev_loop_time = t;    

  // Setting this here to be sure I do not forget to init' it....
  // These initializations should be done by read_settings from eeprom,
  // and this "default model values" should probably be moved
  // out to a section of read_settings when handling "new model", or
  // to a separate model_defaults function...
  model.channels = 8;
  model.rev[0] = model.rev[1] = model.rev[2] = model.rev[3] = 
  model.rev[4] = model.rev[5] = model.rev[6] = model.rev[7] = false;
  model.dr[0] = model.dr[1] = model.dr[2] = model.dr[3] = 0;
  model.dr[4] = model.dr[5] = model.dr[6] = model.dr[7] = 100;

  // Initializing the stopwatch timer/clock values...
  clock_timer = (clock_timer_t){0, 0, 0, false};
}

// ---------- Arduino main loop -----------------------
void loop () {
  
  // Determine if the UI needs to run...
  boolean disp;
  if ( millis() - last > UI_INTERVAL ) { 
    last = millis(); 
    disp = true; 
  }
  else disp = false;

  process_inputs();

  // Wasting a full I/O pin on battery status monitoring!
  battery_val = analogRead(1) * BATTERY_CONV;
  if ( battery_val < BATTERY_LOW ) {
    digitalWrite(13, 1); // Simulate alarm :P
    displaystate = BATTERY;
  }

  if ( disp )
  {
    ui_handler(); 
  }
  if ( displaystate != MENU )
  {
    // Debugging: how long does the main loop take on avg,
    // when not handling the UI...
    t = micros();
    avg_loop_time = ( t - prev_loop_time + avg_loop_time ) / 2;
    prev_loop_time = t;    
  }

  // Whoa! Slow down partner! Let everything settle down before proceeding.
  delay(5);
}

// ----- Simple support functions used by more complex functions ----

void set_ppm_output( bool state )
{
  digitalWrite(A5, state); // Hard coded PPM output
}

void set_timer(long time)
{
  Timer1.detachInterrupt();
  Timer1.attachInterrupt(ISR_timer, time);  
}

boolean check_key( int key)
{
  return ( keys[key] && !prev_keys[key] );
}

void mplx_select(int pin)
{
  digitalWrite(5, 1);
  delayMicroseconds(24);

  digitalWrite(2, bitRead(pin,0));  // Arduino alias for non-modifying bitshift operation
  digitalWrite(3, bitRead(pin,1));  // us used to extract individual bits from the int (0..7)
  digitalWrite(4, bitRead(pin,2));  // Select the appropriate input by setting s1,s2,s3 and e
  digitalWrite(5, 0);               // on the 4051 multiplexer.

  // May need to slow the following read down to be able to
  // get fully reliable values from the 4051 multiplex.
  delayMicroseconds(24);

}

// ----- "Complex" functions follow ---------------------------------

void calibrate()
{
  int i, r0, r1, r2, adc_in;
  int calcount = 0;
  int num_calibrations = 200;

  lcd.clear();
  lcd.print("Move controls to");
  lcd.setCursor(0,1);
  lcd.print("their extremes..");
  Serial.print("Calibration. Move all controls to their extremes.");

  for (i=0; i< MAX_INPUTS; i++) {
    input_cal.min[i] = 1024;
    input_cal.max[i] = 0;
  }
  while ( calcount <= num_calibrations )
  {
    for (i=0; i<=MAX_INPUTS; i++) {
      mplx_select(i);
      adc_in = analogRead(0);

      // Naive min/max calibration
      if ( adc_in < input_cal.min[i] ) {
        input_cal.min[i] = adc_in;
      }
      if ( adc_in > input_cal.max[i] ) {
        input_cal.max[i] = adc_in;
      }
      delay(10);
    }

    calcount++;
  }

  // TODO: WILL need to do center-point calibration after min-max...

  lcd.clear();
  lcd.print("Done calibrating");

  Serial.print("Done calibrating");
  delay(2000);  
}

void read_settings(void)
{
  // Dummy. Will be modified to read model settings from EEPROM
  for (int i=0; i<=7; i++) {
    input_cal.min[i] = 0;
	input_cal.center[i] = 512;
    input_cal.max[i] = 1024;
  }
}

void write_settings(void)
{
  // Dummy. Not used anywhere. Will be fleshed out to save settings to EEPROM.
}

void scan_keys ( void )
{
  int i, r0, r1, r2;
  boolean key_in;

  // To get more inputs, another 4051 analog multiplexer is used,
  // but this time it is used for digital inputs. 8 digital inputs
  // on one input line, as long as proper debouncing and filtering
  // is done in hardware :P
  for (i=0; i<=7; i++) {
    // To be able to detect that a key has changed state, preserve the previous..
    prev_keys[i] = keys[i];

    // Select and read input.
    mplx_select(i);
    keys[i] = digitalRead(A2);
    delay(2);
  }
}


void process_inputs(void )
{
  int current_input, r0, r1, r2, adc_in;
  for (current_input=0; current_input<=7; current_input++) {

    mplx_select(current_input);
    adc_in = analogRead(0);

    model.stick[current_input] = ((float)adc_in - (float)input_cal.min[current_input]) / (float)(input_cal.max[current_input]-input_cal.min[current_input]);    
    if ( model.rev[current_input] ) model.stick[current_input] = 1.0f - model.stick[current_input];  
  }
}


void ISR_timer(void)
{
  Timer1.stop(); // Make sure we do not run twice while working :P

  if ( !do_channel )
  {
    set_ppm_output( LOW );
    sum += seplength;
    do_channel = true;
    set_timer(seplength);
    return;
  }

  if ( cchannel >= model.channels )
  {
    set_ppm_output( HIGH );
    long framesep = framelength - sum;

    sum = 0;
    do_channel = false;
    cchannel = 0;
    set_timer ( framesep );
    return;
  }  

  if ( do_channel )
  {
    set_ppm_output( HIGH );
    long next_timer = (( chwidht * model.stick[cchannel] ) + chmin);
    // Do sanity-check of next_timer compared to chmax ...
    while ( chmax < next_timer ) next_timer--;
    sum += next_timer;

    // Done with channel separator and value,
    // prepare for next channel...
    cchannel++;
    do_channel = false;
    set_timer ( next_timer );
    return;
  }
}

void serial_debug()
{
  int current_input;
  for (current_input=0; current_input<=7; current_input++) {
    int v = (int)(model.stick[current_input] * 100);

    Serial.print("Input #");
    Serial.print(current_input);
    Serial.print(" value: ");
    Serial.print(model.stick[current_input]);
    Serial.print(" pct: ");
    Serial.print(v);
    Serial.print(" min: ");
    Serial.print(input_cal.min[current_input]);
    Serial.print(" max: ");
    Serial.print(input_cal.max[current_input]);
    Serial.println();
  }
  Serial.print("Battery level is: ");
  Serial.println(battery_val);

  Serial.print("Average loop time:");
  Serial.println(avg_loop_time);

  Serial.println();
}
void dr_inputselect( int no, int in )
{
	if ( model.dr[menu_substate] < 0 ) model.dr[menu_substate] = 4;
	if ( model.dr[menu_substate] > 4 ) model.dr[menu_substate] = 0;

	lcd.setCursor(0 , 0);
	lcd.print("D/R switch ");
	lcd.print( no + 1 );
	lcd.print("    ");
	lcd.setCursor(0 , 1);
	lcd.print("Input ");
	lcd.print(in+1);
	
	lcd.print(":  ");
	if ( ! model.dr[menu_substate] ) lcd.print("Off");
	else lcd.print(model.dr[menu_substate]);
	
	if ( check_key(KEY_INC) ) {
		model.dr[menu_substate]++;
		return;
	}
	else if ( check_key(KEY_DEC) ) {
		model.dr[menu_substate]--;
		return;
	}
	// Wrap around.
	return; 

}

void dr_value()
{
	int pos;
	int state;

	if ( menu_substate == 4) state = keys[KEY_DR1];
	else state = keys[KEY_DR2];
	
	pos = 4 + (menu_substate - 4) * 2;
	if (state) pos++;
	
	lcd.setCursor(0 , 0);
	lcd.print("D/R switch ");
	lcd.print( menu_substate - 3 );
	lcd.print("    ");
	lcd.setCursor(0 , 1);
	lcd.print( state ? "HI" : "LO" );
	lcd.print(" Value :");
	
	lcd.print( model.dr[pos] );
	
	if ( keys[KEY_INC] ) {
		if ( model.dr[pos] < 100) model.dr[pos] += 5;
		return;
	}
	else if ( keys[KEY_DEC] ) {
		if ( model.dr[pos] > -100) model.dr[pos] -= 5;
		return;
	}
	
	return;
}
void ui_handler()
{
  int row; 
  int col;
  scan_keys();

  if ( displaystate != MENU )
  {
	menu_substate = 0;
    if ( check_key(KEY_UP) && displaystate == VALUES  ) { 
      displaystate = BATTERY; 
      return; 
    }
    else if ( check_key(KEY_UP) && displaystate == BATTERY ) { 
      displaystate = TIMER; 
      return; 
    }
    else if ( check_key(KEY_UP) && displaystate == TIMER ) { 
      displaystate = VALUES; 
      return; 
    }

    else if ( check_key(KEY_DOWN) ) { 
      displaystate = MENU; 
      return; 
    }
  }

  digitalWrite(13, digitalRead(13) ^ 1 );

  switch ( displaystate )
  {
    case VALUES:
      int current_input;
      for (current_input=0; current_input<=7; current_input++) {
        // In channel value display, do a simple calc
        // of the LCD row & column location. With 8 channels
        // we can fit eight channels as percentage values on
        // a simple 16x2 display...
        if ( current_input < 4 )
        {
          col = current_input * 4; 
          row = 0;
        } 
        else 
        {
          col = (current_input-4) * 4;      
          row = 1;
        }
        // Overwriting the needed positions with
        // blanks cause less display-flicker than
        // actually clearing the display...
        lcd.setCursor(col, row);
        lcd.print("    ");
        lcd.setCursor(col, row);
        // Display uses percents, while PPM uses ratio....
        int v = (int)(model.stick[current_input] * 100);
        lcd.print(v);
      }
      break;


    case BATTERY:    
      lcd.clear();
      lcd.print("Battery level: ");
      lcd.setCursor(0 , 1);
      lcd.print( (float)battery_val/10);
      lcd.print("V");
      if ( battery_val < BATTERY_LOW ) lcd.print(" - WARNING");
      else lcd.print(" - OK");
      break;
      
    

	case TIMER:
		unsigned long delta;
		int hours;
		int minutes;
		int seconds;
		
		lcd.clear();
		lcd.print("Timer: ");
		lcd.print( clock_timer.running ? "Running" : "Stopped" );
		lcd.setCursor(5 , 1);
		if ( clock_timer.running )
		{
			clock_timer.value = millis() - (clock_timer.start + clock_timer.init);
		}
		hours = ( clock_timer.value / 1000 ) / 3600;
		clock_timer.value = clock_timer.value % 3600000;
		minutes = ( clock_timer.value / 1000 ) / 60;
		seconds = ( clock_timer.value / 1000 ) % 60;
		if ( hours ) {
			lcd.print(hours);
			lcd.print(":");
		}
		if ( minutes < 10 ) lcd.print("0");
		lcd.print( minutes );
		lcd.print(":");
		if ( seconds < 10 ) lcd.print("0");
		lcd.print( seconds );
		
        if ( check_key(KEY_INC) ) { 
			if ( !clock_timer.running && !clock_timer.start )
			{
				clock_timer.start = millis();
				clock_timer.value = 0;
				clock_timer.running = true;
			} else if ( !clock_timer.running && clock_timer.start ) {
				clock_timer.start = millis() - clock_timer.value;
				clock_timer.running = true;
			} else if ( clock_timer.running ) {
				clock_timer.running = false;
			}
			return; 
        } else if ( check_key(KEY_DEC) ) { 
			if ( !clock_timer.running && clock_timer.start ) {
				clock_timer.value = 0;
				clock_timer.start = 0;
				clock_timer.init = 0;
			}
			return; 
        }
		break;

    case MENU:
      lcd.clear();
      switch ( menu_mainstate )
      {
        case TOP:
          lcd.print("In MENU mode!");
          lcd.setCursor(0 , 1);
          lcd.print("Esc UP. Scrl DN.");
          menu_substate = 0;
          if ( check_key(KEY_UP) ) { 
            displaystate = VALUES; 
            return; 
          }
          else if ( check_key(KEY_DOWN) ) { 
            menu_mainstate = INVERTS; 
            return; 
          }
          break;
    
    
        case INVERTS:
          if ( menu_substate >= model.channels ) menu_substate = 0;
          if ( menu_substate < 0) menu_substate = (model.channels - 1);
          lcd.print("Channel invert");
          lcd.setCursor(0 , 1);
          lcd.print("Ch ");
          lcd.print(menu_substate+1);
          lcd.print( (model.rev[menu_substate] ?  ": Invert" : ": Normal"));

          if ( check_key(KEY_UP) ) { 
            menu_mainstate = TOP; 
            return; 
          }
          else if ( check_key(KEY_DOWN) ) { 
            menu_mainstate = DUALRATES; 
            return; 
          }

          if ( check_key(KEY_RIGHT) ) { 
            menu_substate++; 
            return; 
          }
          else if ( check_key(KEY_LEFT) ) { 
            menu_substate--; 
            return; 
          }
          else if ( check_key(KEY_INC) || check_key(KEY_DEC) ) { 
            model.rev[menu_substate] ^= 1; 
            return; 
          }
          break;
          
        case DUALRATES:
          if ( menu_substate > 5 ) menu_substate = 0;
          if ( menu_substate < 0) menu_substate = 5;
		
          if ( check_key(KEY_UP) ) { 
            menu_mainstate = INVERTS; 
            return; 
          }          
          if ( check_key(KEY_DOWN) ) {
            menu_mainstate = EXPOS;
            return;
          }
          if ( check_key(KEY_RIGHT) ) { 
            menu_substate++; 
            return; 
          }
          else if ( check_key(KEY_LEFT) ) { 
            menu_substate--; 
            return; 
          }
          switch (menu_substate)
		  {
			case 0:
				dr_inputselect(0, 0);
				return;
			case 1:
				dr_inputselect(0, 1);
				return;
			case 2:
				dr_inputselect(1, 0);
				return;
			case 3:
				dr_inputselect(1, 1);
				return;
			case 4:
			case 5:
				dr_value();
				return;
			default:
				menu_substate = 0;
				break;
		  }		  
          break;
        
        case EXPOS:
                   //________________
          lcd.print("Input expo curve");
          lcd.setCursor(0 , 1);
          lcd.print("Not implemented");
          // Possible, if input values are mapped to +/- 100 rather than 0..1 ..
          // plot ( x*(1 - 1.0*cos (x/(20*PI)) )) 0 to 100
          // Run in wolfram to see result, adjust the 1.0 factor to inc/red effect.
          // Problem: -100 to 100 is terribly bad presicion, esp. considering that
          // the values started as 0...1024, and we have 1000usec to "spend" on channels.
          if ( check_key(KEY_UP ) ) { 
            menu_mainstate = DUALRATES; 
            return; 
          }          
          if ( check_key(KEY_DOWN ) ) {
            menu_mainstate = DEBUG;
            return;
          }
          break;
          
        case DEBUG:
          lcd.setCursor(0 , 0);
          lcd.print("Dumping debug to");
          lcd.setCursor(0 , 1);
          lcd.print("serial port 0");
          serial_debug();          
          if ( check_key(KEY_UP ) ) { 
            // FIXME: Remember to update the "Scroll up" state!
            menu_mainstate = EXPOS; 
            return; 
          } else if ( check_key(KEY_DOWN ) ) {
            menu_mainstate = SAVE;
            return;
          }
          break;
    
        default:
          lcd.print("Not implemented");
          lcd.setCursor(0 , 1);
          lcd.print("Press DOWN...");
          if ( check_key(KEY_DOWN ) ) menu_mainstate = TOP;
      }
      break;
      
      
    default:
      // Invalid
      return;
  }

  return;
}