#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <Arduino.h>

#define servo_clk_prescalar 0
#define trigger_pin 7
#define echo_pin 8

void initialiseHSR04Ports(void);
void initialiseServoPorts(void);
unsigned long pwm_icr1(uint8_t clk_prescalar);
void trigger_pulse();
float measure_hsr04();
float map_range(float x, float in_min, float in_max, float out_min, float out_max);


int main(void){
  Serial.begin(9600);

  initialiseHSR04Ports();
  initialiseServoPorts();

  unsigned long limit = pwm_icr1(servo_clk_prescalar);
  ICR1 = limit;

  TIMSK1 = TIMSK1 | (1 << ICIE1); //Input capture IT enable
  TCCR1B = TCCR1B | (1 << ICES1); //Input capture edge select (rising edge)
  sei();
	

  while(1){
    float distance = measure_hsr04();

    float servo_pulse = map_range(distance, 15, 100, 0.5, 1.5);
    OCR1A = (servo_pulse/20) * limit;
  }


  return 0;

}


void initialiseHSR04Ports(void){
	DDRD |= (1 << 7);						//PORTD.7 = INPUT ... triggerpin
}

void initialiseServoPorts(void){
	DDRB |= 1 << PINB1;
  TCCR1A |= (1 << WGM11) | (1 << COM1A1);
  TCCR1B |= (1 << WGM13) | (1 << WGM12);
}


unsigned long pwm_icr1(uint8_t clk_prescalar){
  unsigned long N;
  switch (clk_prescalar)
    {
    case 8:
        TCCR1B |= (1 << CS11);
        N = 40000;
        break;
    case 64:
        TCCR1B |= (1 << CS11) | (1 << CS10);
        N = 5000;
        break;
    case 256:
        TCCR1B |= (1 << CS12);
        N = 1250;
        break;
    // case 1024 ICR1 = 312.5 which isn't an integer
    default:
        TCCR1B |= (1 << CS10);
        N = 320000;
        break;
    }

    return N;
}


void trigger_pulse(){
  PORTD |= (1 << 7);
	_delay_us(10);
	PORTD &= (~(1 << 7));
}


float measure_hsr04(){
    _delay_us(10);    // wait 10 us before sending trigger pulse again
    trigger_pulse();

    float d = pulseIn(8, HIGH);
    return d / 58.8235;
}


//                            15            100            0            90
float map_range(float x, float in_min, float in_max, float out_min, float out_max){
    if(in_min > x){ 
        return out_min; 
    }
    if(in_max < x){ 
        return out_max; 
    }

    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}





// ISR(TIMER1_CAPT_vect){
//   Serial.println("entered interupt");
// 	if(captured == 1)
// 	{

// 		time1 = ICR1; //Save the rising edge of the signal to time1 variable

// 		TCCR1B = TCCR1B & ~(1 << ICES1); //Input capture edge select (falling edge)

// 		captured = captured + 1;

// 	}

// 	else

// 	{

// 		time2 = ICR1; //Save the fallling edge of the signal to time2 variable

// 		captured = 1;

// 	}

//   	/*Stop and reset TIMER1*/

// 	TCCR1B = TCCR1B & ~(1 << CS11);

// 	TCCR1B = TCCR1B & ~(1 << CS10); //Stop TIMER1

// 	TIMSK1 = TIMSK1 & ~(1 << ICIE1); //Input capture IT disable

// 	TCNT1 = 0; //Reset TIMER1

 

// 	/*Compute distance*/

// 	finalTime = time2 - time1;

// 	distance = ((finalTime*0.000004*340)/2); //distance = (pulsewidth*c)/2 where c = 340 m/s and pulsewidth is measured in seconds

// 	trig = 1; //Set the trigger flag to signal the measurement is over
// }





    // Serial.println("hello");
    // _delay_us(10);
    // trigger_pulse();
		
		// TCNT1 = 0;	/* Clear Timer counter */
		// TCCR1B = 0x41;	/* Capture on rising edge, No prescaler*/
		// TIFR1 = 1<<ICF1;	/* Clear ICP flag (Input Capture flag) */
		// TIFR1 = 1<<TOV1;	/* Clear Timer Overflow flag */

		// /*Calculate width of Echo by Input Capture (ICP) */
		
		// while ((TIFR1 & (1 << ICF1)) == 0);/* Wait for rising edge */
		// TCNT1 = 0;	/* Clear Timer counter */
		// TCCR1B = 0x01;	/* Capture on falling edge, No prescaler */
		// TIFR1 = 1<<ICF1;	/* Clear ICP flag (Input Capture flag) */
		// TIFR1 = 1<<TOV1;	/* Clear Timer Overflow flag */
		// TimerOverflow = 0;/* Clear Timer overflow count */

		// while ((TIFR1 & (1 << ICF1)) == 0);/* Wait for falling edge */
		// int count = ICR1 + (65535 * TimerOverflow);	/* Take count */
		// /* 8MHz Timer freq, sound speed =343 m/s */
		// float distance = (float)count / 466.47;
    

    // float servo_pulse = map_range(distance, 15, 100, 0.5, 1.5);

    // OCR1A = (servo_pulse/20) * limit;