#include <stdio.h>
#include <stdlib.h>
#include "math.h"
#include "SDlib16.h"
#include "./configbits-16ex8.h"

unsigned short I2Crx_buffer;
int data = 5;      //data variable
int dIndex; 
int check_RW;   //holds read/write bit
int check_DA;
int read_write;   //enter into read_write loop when == 1
int test_data = 1;  //dummy data to test program
int address = 0x05;   //address of the PIC32
int interruptcheck = 0;

#define Read_Write I2C3STATbits.R_W
#define Data_Addr  I2C3STATbits.D_A
#define interruptcheck IFS0bits.I2C3SIF


double trilaterationX(double p1[], double p2[], double p3[], double r[])
{
    double xCoor = 0;
    double d = p2[0] - p1[0]; //Find x magnitude between pylon 1 and pylon 2
    xCoor = (pow(r[0], 2) - pow(r[1], 2) + pow(d, 2)) / (2*d); //Gives us the x coordinate of the WR, formula from wiki's trilat

    return xCoor; //Return the x coordinate
}

double trilaterationY(double p1[], double p2[], double p3[], double r[], double xCoor)
{
    double yCoor = 0;

    double i = p3[0] - p1[0]; //Find x magnitude between pylon 1 and pylon 3
    double j = p3[1] - p1[1]; //Find y magnitude between pylon 1 and pylon 3

    double p = (pow(r[0], 2) - pow(r[2], 2) + pow(i, 2) + pow(j, 2)) / (2*j); //Temp variable for first part of the equation from wiki
    yCoor = p - ((i / j) * xCoor); //Gives us the y coordinate of the WR's position
    yCoor = 53 - yCoor; //Give us the value for our orientation if we were facing the ND sideline

    return yCoor;
}

int main()
{
    //I2C Initialization
    I2C3CONbits.ON = 1;
    I2C3CONbits.A10M = 0; //sets I2CxADD to 7 bit address mode
    I2C3ADD = address; //first set address of slave to 0000000 in binary
    
    double coorWR1[] = {0,0}; //Initialize WR1
    double coorWR2[] = {0,0}; //Initialize WR2
    double coorWR3[] = {0,0}; //Initialize WR3

    double p1[] = {0, 0}; //Pylon 1 position, top left corner of the end zone on field schematic
    double p2[] = {60, 0}; //Pylon 2 position, on the 50 on the ND side on field schematic
    double p3[] = {0, 53}; //Pylon 3 position, bottom left corner of the end zone on the field schematic

    double wr1[] = {35.269, 39.263, 41.378}; //Radii from the pylons to WR 1 w.r.t. the pylons from the field schematic
    //double wr2[] = {38.328, 25.080, 56.727}; //Radii from the pylons to WR 2 w.r.t. the pylons from the field schematic

    double xCoor = trilaterationX(p1, p2, p3, wr1); //Initialize WR1 x coordinate
    double yCoor = trilaterationY(p1, p2, p3, wr1, xCoor); //Initialize WR1 y coordinate
    coorWR1[0] = xCoor; //Set the x value in the array
    coorWR1[1] = yCoor; //Set the y value in the array
    
    //loop infinitely
    while(1){
        xCoor = trilaterationX(p1, p2, p3, wr1); //Initialize WR1 x coordinate
        yCoor = trilaterationY(p1, p2, p3, wr1, xCoor); //Initialize WR1 y coordinate
        coorWR2[0] = xCoor; //Set the x value in the array
        coorWR2[1] = yCoor; //Set the y value in the array
            
        if ((interruptcheck == 1) && (Read_Write == 0) && (Data_Addr == 0)){
             //(0)INPUT from Master  (0) ADDRESS bit
            //if(IFS0bits.I2C3MIF == 1){  //interrupt if address matches
                I2Crx_buffer = I2C3RCV; //do a dummy read
                //delay_ms(1);    //1ms delay            
                I2C3CONbits.SCLREL = 1; //release clock
            //}
        }
        else if ((interruptcheck == 1) && (Read_Write == 0) && (Data_Addr == 1)){
         //(0)INPUT from Master       (1) DATA bit
            //if(I2C3STATbits.ACKSTAT == 0){  //if ACK recieved
                data = I2C3RCV; //get data from receive buffer
                //delay_ms(1);    //1ms delay
                I2C3CONbits.SCLREL = 1; //release clock
            //}
        }
        else if ((interruptcheck == 1) && (Read_Write == 1) && (Data_Addr == 0)){
         //(1)OUTPUT from Slave       (0) ADDRESS bit
            //if(IFS0bits.I2C3MIF == 1){
                I2C3CONbits.SCLREL = 0;
                I2Crx_buffer = I2C3RCV; //do a dummy read
                I2C3TRN = data; //load transmit buffer with test data
                delay_ms(5);    //1ms delay
                I2C3CONbits.SCLREL = 1; //release clock
                dIndex = 0;
                
            //}
        }
        else if ((interruptcheck == 1) && (Read_Write == 1) && (Data_Addr == 1)){
         //(1)OUTPUT from Slave       (1) DATA bit

            if (dIndex == 0) {
                I2C3TRN = data; //load transmit buffer with test data
                delay_ms(5);    //1ms delay
                I2C3CONbits.SCLREL = 1;
                dIndex++;
            }
            //interruptcheck = 0;
        }
        if(I2C3STATbits.ACKSTAT == 1){ //1 means ACK not recieved, no more data required.
            
        }
    }
    return 0;
}