#include "movement.h"
#include <stdint.h>
#include <stdbool.h>
#include "timer.h"
#include "lcd.h"
#include <inc/tm4c123gh6pm.h>
#include "driverlib/interrupt.h"
#include <string.h>
#include "uart.h"
#include "adc.h"
#include <math.h>
#include "ping.h"

unsigned int startTime = 0;
unsigned int endTime = 0;

unsigned int clockCycles = 0;

char isHighVoltage = 0;

void ping_init(void) {

    // Port B is used for timer 3B
    SYSCTL_RCGCGPIO_R |= 0b000010;

    GPIO_PORTB_DEN_R |= 0b00001000;

    GPIO_PORTB_DIR_R |= 0b00001000;

    // Set the pulse to low voltage
    GPIO_PORTB_DATA_R &= 0b11110111;

    SYSCTL_RCGCTIMER_R |= 0b001000;

    TIMER3_CTL_R &= ~0x0100;

    TIMER3_CFG_R |= 0b100;
    TIMER3_CFG_R &= 0xFFFFFFFC;

    // Count up, edge-time, capture mode
    TIMER3_TBMR_R = 0b10111;

    TIMER3_CTL_R |= 0b110000000000;

    TIMER3_TBPR_R |= 0x00FF;

    TIMER3_TBILR_R |= 0xFFFF;

    TIMER3_ICR_R |= 0b010000000000;

    // Enable capture event interrupts
    TIMER3_IMR_R |= 0b010000000000;

    NVIC_PRI9_R |= 0x00000020;

    NVIC_EN1_R |= 0x0010;

    IntRegister(52, TIMER3B_handler);

    TIMER3_CTL_R |= 0x0100;

    IntMasterEnable();
}

void TIMER3B_handler(void) {

    if (TIMER3_MIS_R & 0b010000000000) {

        if (!isHighVoltage) {

            startTime = TIMER3_TBR_R;
        }
        else {

            endTime = TIMER3_TBR_R;

            if (endTime < startTime) {

                endTime += 0x00FFFFFF;
            }

            clockCycles = endTime - startTime;
        }

        // Clear the interrupt
        TIMER3_ICR_R |= 0b010000000000;

        // Flip the value of the boolean for the interrupt handler.
        isHighVoltage = ~isHighVoltage;
    }
}
void ping_read(void) {

    GPIO_PORTB_PCTL_R &= 0x00000000;

    // Disable the alternate function
    GPIO_PORTB_AFSEL_R &= 0b11110111;

    // Set PB3 as an output
    GPIO_PORTB_DIR_R |= 0b00001000;

    // Create a starting pulse
    GPIO_PORTB_DATA_R |= 0b00001000;

    timer_waitMicros(5);

    GPIO_PORTB_DATA_R &= 0b11110111;

    // Enable the alternate function
    GPIO_PORTB_AFSEL_R |= 0b00001000;

    // Use function T3CCP1
    // See table 23-5 for functions
    GPIO_PORTB_PCTL_R |= 0x00007000;

    // Set PB3 as an input
    GPIO_PORTB_DIR_R &= 0b11110111;
}

double ping_pulse_time(int clockCycles) {

    double time = clockCycles * 6.25 * pow(10, -5);

    return time;
}
double ping_pulse_distance(double time_millis) {

    double distance = time_millis * 34 / 2;

    return distance;
}

double getClockCycles(){
    return clockCycles;
}

#include "movement.h"
#include <stdint.h>
#include <stdbool.h>
#include "timer.h"
#include "lcd.h"
#include <inc/tm4c123gh6pm.h>
#include "driverlib/interrupt.h"
#include <string.h>
#include "uart.h"
#include "adc.h"
#include <math.h>
#include "ping.h"
#include "servo.h"
#include "button.h"
#include "servo.h"


void main()
{
    double angleCounter = 0; //angle starts at 0
    init_Servo(); //initialization
    adc_init(); //initialize adc
    uart_init(); //initialize UART
    lcd_init(); //initialization
    ping_init();
    int dataValue = 0; //IR VALUE
    char unConvert[100];//array that stores the number values
    char pingConvert[100];
    char spaces[12] = "         ";
    char template[100] = "Degrees IR Distance (cm) Sonar Distance (cm)";
    move_servo((int) angleCounter);
    char degreeConvert[100];
    timer_waitMillis(400);
    uart_sendStr(&template);
    uart_sendChar('\n');//new line
    uart_sendChar('\r');
    while (1)
    {
        while (angleCounter <= 180)
        {
            move_servo((int) angleCounter);
            dataValue = adc_read(); //get the value from the sensor
            ping_read();
            double time = ping_pulse_time(getClockCycles());
            double distance = ping_pulse_distance(time);
            timer_waitMillis(500);
            int ConvertIRtoCM = 125718 * (pow(dataValue, -1.185));
            sprintf(pingConvert, "%.2f", distance);
            sprintf(unConvert, "%d", ConvertIRtoCM); //store values into sprintf
            sprintf(degreeConvert, "%.1f", angleCounter);
            uart_sendStr(&degreeConvert);
            uart_sendStr(&spaces);
            uart_sendStr(&unConvert); //send the centimeters to putty
            uart_sendStr(&spaces);
            uart_sendStr(&pingConvert);
            uart_sendChar('\n');//new line
            uart_sendChar('\r');
            angleCounter += 2;
        }
    }

}


#ifndef PING_H_
#define PING_H_

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <inc/tm4c123gh6pm.h>
#include "timer.h"
#include <math.h>

void ping_init(void);
void ping_read(void);
void TIMER3B_handler(void);
double ping_pulse_time(int clockCycles);
double ping_pulse_distance(double time_millis);
double getClockCycles();

#endif /* PING_H_ */