// servosquirter.c
// for NerdKits with ATmega168
// mrobbins@mit.edu

#define F_CPU 14745600

#include <stdio.h>

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

#include "../libnerdkits/delay.h"
#include "../libnerdkits/lcd.h"
#include "../libnerdkits/uart.h"

// PIN DEFINITIONS:
//
// PB4 - Push button input
// PB3 - continuous motor control
// PB2 - PWM output (OC1B)

//temp sensor code - for later use with PWM
void adc_init() {
  // set analog to digital converter
  // for external reference (5v), single ended input ADC0
  ADMUX = 0;

  // set analog to digital converter
  // to be enabled, with a clock prescale of 1/128
  // so that the ADC clock runs at 115.2kHz.
  ADCSRA = (1<<ADEN) | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);

  // fire a conversion just to get the ADC warmed up
  ADCSRA |= (1<<ADSC);
}

uint16_t adc_read() {
  // read from ADC, waiting for conversion to finish
  // (assumes someone else asked for a conversion.)
  // wait for it to be cleared
  while(ADCSRA & (1<<ADSC)) {
    // do nothing... just hold your breath.
  }
  // bit is cleared, so we have a result.

  // read from the ADCL/ADCH registers, and combine the result
  // Note: ADCL must be read first (datasheet pp. 259)
  uint16_t result = ADCL;
  uint16_t temp = ADCH;
  result = result + (temp<<8);

  // set ADSC bit to get the *next* conversion started
  ADCSRA |= (1<<ADSC);
  
  return result;
}

double sampleToFahrenheit(uint16_t sample) {
  // conversion ratio in DEGREES/STEP:
  // (5000 mV / 1024 steps) * (1 degree / 10mV)
  //	^^^^^^^^^^^		 ^^^^^^^^^^
  //     from ADC		  from LM34
  return sample * (5000.0 / 1024.0 / 10.0);  
}
void pwm_set(uint16_t x) {
  OCR1B = x;
}


#define PWM_MIN 0
#define PWM_MAX 512
#define PWM_START 150
void pwm_init() {
  // setup Timer1 for Fast PWM mode, 16-bit
  // COM1B1 -- for non-inverting output
  // WGM13, WGM12, WGM11, WGM10 -- for Fast PWM with OCR1A as TOP value
  // CS10 -- for no clock prescaling
  
  OCR1A = 512;// set for 28.8Khz cycle
  pwm_set(PWM_START);
  TCCR1A = (1<<COM1B1) | (1<<WGM11) | (1<<WGM10);
  TCCR1B = (1<<WGM13) | (1<<WGM12) | (1<<CS10);
  
}

//Debounce module
//written by hevans
//Debounce will return 
//
//0 if there was a debounced high -> low transition
//1 if there was a debounced low -> high transition
//2 if there was no valid transition
//
//Note: this debounce module does not rely on real time, you have to call it in your
//main loop so its internal counter will count down. Implelemting a timer driven
//version of this module might be a fun challenge.
//
//This module has non-repeating behavior, a conitnuous press will only return one transition
#define DB_DELAY 10
uint8_t debounce_curr_state;
uint8_t debounce_delay;
void debounce_init(){
  debounce_curr_state = PINB & (1<<PB4);
  debounce_delay = DB_DELAY;
}

uint8_t debounce_button(){

  uint8_t button_state = PINB & (1<<PB4); //sets which this module listens to

  if(button_state == debounce_curr_state){ //same button state
    if(debounce_delay > 0) {
      debounce_delay--;
      return 2;
    } else
      return 2;
  } else { //change in button state
    if(debounce_delay > 0){ //not enought time elapsed do not change state
      debounce_delay--;
      return 2;
    } else { //enough time elapsed, valid transition
      debounce_delay = DB_DELAY;
      debounce_curr_state = button_state;
      if(button_state){
        return 1;
      }
      else 
        return 0;
    }
  }
}

//Main loop handles the two modes of operation on two different pins
//PB2 is a PWM output whose pulse width is proportional to temperature.
//PB3 is a digital pin that can be on or off and will toggle with a push button
//PB4 is the push botton input
int main() {
  // init LCD
  lcd_init();
  FILE lcd_stream = FDEV_SETUP_STREAM(lcd_putchar, 0, _FDEV_SETUP_WRITE);
  lcd_write_string(PSTR(" Motor Control "));
  
  // init serial port
  uart_init();
  FILE uart_stream = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
  stdin = stdout = &uart_stream;

  pwm_init();
  adc_init();
  debounce_init();

  DDRB |= (1<<PB2) | (1<<PB3); // set PB2,PB3 as output
  DDRB &= ~(1<<PB4); // set PB4 as input
  PORTB |= (1<<PB4); // turn on pullup resistor for PB4
  
  uint16_t pos = PWM_START;

  uint8_t motor_state = 0;
  uint8_t temp_button_state;

// holder variables for temperature data
  uint16_t last_sample = 0;
  double this_temp;
  double temp_avg;
  uint8_t i;
  temp_avg = 0.0;
  for(i=0; i<200; i++) {
    last_sample = adc_read();
    this_temp = sampleToFahrenheit(last_sample);

    // add this contribution to the average
    temp_avg = temp_avg + this_temp/200.0;
  }
  double room_temp = temp_avg;
  double pos_temp;
  double scale = 34.0;  //this many PWM steps per 1 degree difference in temp
  while(1) {
    
    //check the pusbutton
    temp_button_state = debounce_button();
    if(temp_button_state != 2){
      if(temp_button_state == 0){
        if(motor_state)
          motor_state = 0;
        else
          motor_state = 1;
      }
    }

    if(motor_state){
      PORTB |= (1<<PB3);
    } else {
      PORTB &= ~(1<<PB3);
    }

    
    // take 100 samples and average them!
    temp_avg = 0.0;
    for(i=0; i<100; i++) {
      last_sample = adc_read();
      this_temp = sampleToFahrenheit(last_sample);

      // add this contribution to the average
      temp_avg = temp_avg + this_temp/100.0;
    }
    pos_temp = PWM_START + scale*(temp_avg - room_temp); //calculate new PWM value
    pos = pos_temp;
     // bounds checking
    if(pos > PWM_MAX) pos = PWM_MAX;
    if(pos < PWM_MIN) pos = PWM_MIN;
    
    pwm_set(pos);  
    
    //write into to the LCD
    lcd_line_two();
    fprintf_P(&lcd_stream, PSTR("Continuous State: %d "), motor_state);
    lcd_line_three();
    fprintf_P(&lcd_stream, PSTR("Temperature: %.2f  "), temp_avg);
    lcd_line_four();
    fprintf_P(&lcd_stream, PSTR("PWM: %d of 512  "), pos);



 } 

  return 0;
}