/* 
 * File:  main.c
 * Project: Helmet.X
 * Author: Claire Chen and Mark Zhao
 */
 
//=========================Header/Definitions==============================

#define	SYS_FREQ 40000000 // change the frequency of the clock
#include "nrf24l01.h"
// These two defines are for the tft
#define spi_channel	1
#define spi_divider 10
#define PACKET_SIZE 4
//#define PACKET_SIZE 1
//Config file
#include "config.h"
// threading library
#include "pt_cornell_1_2.h"


////////////////////////////////////
// graphics libraries
#include "tft_gfx.h"
#include "tft_master.h"
////////////////////////////////////
#include <stdio.h>
#include "sound.h"

#define MAX_DISTANCE 360.0 //cm


//==========================================================================
//===============================Global Variables===========================
static struct pt pt_sonar;
char buffer[120]; // a buffer for writing to the tft
volatile char tx_packet[PACKET_SIZE]; // synchronous counter
volatile char rx_packet[PACKET_SIZE];

// Sign table
signed int sin_table[sine_table_size];

volatile unsigned int phase_accum_l = 0;
volatile unsigned int sine_l;
volatile unsigned int phase_accum_r = 0;
volatile unsigned int sine_r;
volatile unsigned int phase_incr;
volatile uint16_t sine_data_l; // blue wire
volatile uint16_t sine_data_r;
volatile int tone_duration_r = 0; // Keeps track of tone playback duration
volatile int tone_duration_l = 0; 
volatile int tone_flag;  //1 if the DTMF tone is to be played
float sound_amp;
float diff_amp_r;
float diff_amp_l;
int degree = 0;
int time_diff = 0;
int phase_diff = 0;

//===============================Initialization Functions===================
/*
 * Initialize protothreads and enable interrupts
 */
void init_pthreads(void){
  // === config threads ==========
  // turns OFF UART support and debugger pin, unless defines are set
  PT_setup();
  PT_INIT(&pt_sonar);
}

// set up the radio
void radioSetup() {
    nrf_setup(); // initializing function

    nrf_set_arc(0x0A); // 10 retransmits
    
    nrf_set_ard(0x00); // 250 us between retransmission

    nrf_set_rf_ch(0x01); // freq = 2.401 GHz

    nrf_en_aa(0); // enable autoack on pipe 0

    nrf_set_pw(PACKET_SIZE, 0); //set the payload width to be 1 byte

    // set up addresses for autoack mode
    // The tx address and the pipe 0 rx address must be the same for autoack mode
    nrf_set_address_width(5);
    nrf_set_rx_addr(0, 0xAABBCCDDFF, 5);
    nrf_set_tx_addr(0xAABBCCDDFF);
}

// calculate sound parameters to be used in ISR
void play_sound(unsigned int degree, unsigned int distance){
  tone_flag = 1;
  time_diff = (int) (MAX_HEAD_DELAY_us * cos(degree*PI/180.0));
  phase_diff = (int) (((time_diff * sound_freq) / 1000000.0) * two32);
  tone_duration_r = 0;
  tone_duration_l = 0;
  
  if (distance <= MAX_DISTANCE && distance >= 0){
    sound_amp = -1.0 * (distance/MAX_DISTANCE) + 1;
  }
  else{
    sound_amp = 1;
  }
  
  if(degree <= 90){
    diff_amp_l = 1.0;
    diff_amp_r = 0.5 * sin(degree*PI/180.0) + 0.5;
  }
  else{
    diff_amp_l = 0.5 * sin(degree*PI/180.0) + 0.5;
    diff_amp_r = 1.0;
  }
}

void stop_sound(void){
  tone_flag = 0;
}

//==========================================================================
//=========================== Threads ======================================

static PT_THREAD (protothread_sonar(struct pt *pt)) {
    PT_BEGIN(pt);
    tft_setRotation(0); // Use tft_setRotation(1) for 320x240
    char buffer[120]; // a buffer for writing to the tft
    static unsigned int dist_cm;
    static unsigned int angle;
    
    while (1) {
        // set radio to receive mode to start
        nrf_state_rx_mode();
        // clear the screen
        tft_fillScreen(ILI9340_BLACK);
        // display "Current Count:" at top of screen
        tft_setTextColor(ILI9340_GREEN);
        tft_setTextSize(2);
        tft_setCursor(20, 0);
        sprintf(buffer, "%s", "Current Count:");
        tft_writeString(buffer);
        // wait until a payload is received before doing anything
        if (nrf_payload_available()) { // when a payload has been received
            tft_fillScreen(ILI9340_BLACK);
            // get new counter value from transmitter
            nrf_get_payload(rx_packet, PACKET_SIZE);

                  //Check the packet
      
            if(rx_packet[0] == 'd'){
              angle = rx_packet[1];
              dist_cm = (rx_packet[2] << 8) | rx_packet[3];

              //Play the sound
              play_sound(angle, dist_cm);
              //PT_YIELD_TIME_msec(100);
            }
            else{
              //do nothing
            }
            
            // display counter value
            tft_setCursor(20, 20);
            sprintf(buffer, "%d", rx_packet[0]);
            tft_writeString(buffer);

            // send incremented payload
            nrf_send_payload(rx_packet, PACKET_SIZE); // send counter back to other radio
            nrf_state_rx_mode(); // put the radio in rx mode after the transmission
        }
    }
    PT_END(pt);
} 

// DDS units for sound localization
void __ISR(_TIMER_2_VECTOR, ipl2) Timer2Handler(void)
{
  mT2ClearIntFlag(); // Clear timer 2 interrupt flag

  //If tone flag is set, then output sound
  if (tone_flag == 1){
      phase_accum_r = phase_accum_r + phase_incr;
      phase_accum_l = phase_accum_r + phase_diff;
      sine_r = sin_table[phase_accum_r >> 24]; // Table lookup
      sine_l = sin_table[phase_accum_l >> 24]; // Table lookup
      sine_data_l = sine_l + 2048; 
      sine_data_r = sine_r + 2048;

      // LEFT envelope ramping, MAX_TONE_DURATION_ms pulse length total
      if (tone_duration_l <= 0 || tone_duration_l >= MAX_TONE_DURATION_us){
        sine_data_l = 0;
      }
      else if (tone_duration_l < 8000){
        //linear ramp up to amplitude 1
        sine_data_l = (int) ( (tone_duration_l/8000.0) * sine_data_l);
      }
      else if (tone_duration_l > (MAX_TONE_DURATION_us - 8000)){
        sine_data_l = (int) (((MAX_TONE_DURATION_us-tone_duration_l)/8000.0) * sine_data_l); 
      }
      
      // RIGHT envelope
      if (tone_duration_r <= 0 || tone_duration_r >= MAX_TONE_DURATION_us){
        sine_data_r = 0;
      }
      else if (tone_duration_r < 8000){
        //linear ramp up to amplitude 1
        sine_data_r = (int) ( (tone_duration_r/8000.0) * sine_data_r);
      }
      else if (tone_duration_r > (MAX_TONE_DURATION_us - 8000)){
        sine_data_r = (int) (((MAX_TONE_DURATION_us-tone_duration_r)/8000.0) * sine_data_r); 
      }
     
      sine_data_r = (int) (sine_data_r * sound_amp * diff_amp_r);
      sine_data_l = (int) (sine_data_l * sound_amp * diff_amp_l);
      
      // write dtmf_data to DAC channels
      DAC_write(DAC_config_chan_A | (sine_data_l));
      DAC_write(DAC_config_chan_B | (sine_data_r));
    
    // Update tone_duration by isr period in us
    tone_duration_l = tone_duration_l + 62;  //ISR entered every 2500 cycles = 62.5us at 40mHz clock
    tone_duration_r = tone_duration_l - time_diff; 
  }
}

void main(void) {
  INTEnableSystemMultiVectoredInt();
  init_pthreads();
  radioSetup();  
     
  DAC_init();
  config_sound_timer();
 
  int i;
  // Initialize sine lookup table
  for (i = 0; i < sine_table_size; i++){
    sin_table[i] = (int)(2047*sin((float)i*6.283/(float)sine_table_size));
  }
  
  // Set phase increment to generate 440 Hz sine wave
  phase_incr = ((sound_freq / Fs) * two32);
    
  while(1){
    PT_SCHEDULE(protothread_sonar(&pt_sonar));
  }

} // main
// === end  ======================================================