#include "mbed.h"
#include "MMA7660.h"
#include "C12832.h"
#include "LM75B.h"
#include "main.h"
//#include "MSCFileSystem.h"

//Declare LEDs
DigitalOut blue_led(p25);
DigitalOut red_led(p23);

//Declare Speaker
PwmOut spkr(p26);

//Declare LED
C12832 lcd(p5, p7, p6, p8, p11);

//Accelerometer
MMA7660 MMA(p28, p27);
//Temp
LM75B sensor(p28,p27);

//Potentiometers
AnalogIn vibPot(p19);
AnalogIn tempPot(p20);

Ticker red_ticker;
Ticker blue_ticker;
Ticker spkr_ticker;

static const float fAccelBound = 1.5;
static const float fTempBound = 50;
static const int VOLUME = 250;

float fCurrentTemp;

float fZaxis_p;
float fZaxis_n;

float fYaxis_p;
float fYaxis_n;

float fXaxis_p;
float fXaxis_n;

char cAxisID;

float fAxisSenseThresh = 0.2;

float fGravity = 1.0;

float fVibrationThresholdReached = 0;

int main(){
    blue_led = 1;
    red_led = 1;

    float fPosVibThresh = 0.0;
    float fNegVibThresh = 0.0;

    while(1){

        if(fVibrationThresholdReached == 0){
            fPosVibThresh = vibPot * fAccelBound;
            fNegVibThresh = -fNegVibThresh;
            LCD_Stat_Update(fPosVibThresh);

            fZaxis_p = MMA.z();
            fZaxis_n = -MMA.z();

            fXaxis_p = MMA.x();
            fXaxis_p = -MMA.x();

            fYaxis_p = MMA.y();
            fYaxis_p = -MMA.y();

            if((fZaxis_p >= fPosVibThresh && fZaxis_n <= fNegVibThresh) && (fPosVibThresh > fGravity && fNegVibThresh < -fGravity)){
                cAxisID = 'Z';
            }

            else if(fYaxis_p >= fPosVibThresh && fZaxis_n <= fNegVibThresh){
                cAxisID = 'Y';
            }

            else if(fXaxis_p >= fPosVibThresh && fXaxis_n <= fNegVibThresh){
                cAxisID = 'X';
            }

            else{
                cAxisID = NULL;
            }

            switch(cAxisID){
                case 'Z':
                case 'Y':
                case 'X':
                    LCD_Vibration_Update();
                    fVibrationThresholdReached = 1;
                    Vibration_Threshold_Reached();
                    break;
                case NULL:

                    break;
            }

        }

    }



}

void LCD_Stat_Update(float fPosVibThresh){
    lcd.cls();
    lcd.locate(0,3);

    //Displays the Vibration Threshold to 2 decimal places.
    lcd.printf("Vibration Threshold: %.2f", fPosVibThresh);

    wait(0.2);
}

void LCD_Temp_Update(float fTempThreshold, float fCurrentTemp){
    lcd.cls();
    lcd.locate(0,3);
    lcd.printf("Temp Threshold: %.2f\nTemperature:%.2f ->", fTempThreshold, fCurrentTemp);
    wait(0.2);
}

void LCD_Vibration_Update(){
    lcd.cls();
    lcd.locate(0,3);
    lcd.printf("Vibration Threshold Reached\nOn Axis: %c", cAxisID);
    wait(0.2);
}

void LCD_Armageddon_Update(){
    lcd.cls();
    lcd.locate(10,7);
    lcd.printf("   !!Gloom and Doom!!");
    }

void Vibration_Threshold_Reached(){
    int iFlashController;

    float fTempThreshold;

    bool isBlueSet = false;

    MMA.setActive(false);

    while(1){
        fCurrentTemp = sensor.read();
        fTempThreshold = tempPot * fTempBound;

        if(fCurrentTemp >= fTempThreshold){
            iFlashController = 2;
            Led_Control(iFlashController);
            LCD_Armageddon_Update();
            break;
        }
        else{
            if(!isBlueSet){
                iFlashController = 1;
                Led_Control(iFlashController);
                isBlueSet = true;
            }
        }

        LCD_Temp_Update(fTempThreshold, fCurrentTemp);
    }
}

void Led_Control(int iFlashController){
    switch(iFlashController){
        case 1:// Blue LED Flashing @1Hz, Red LED off
            blue_ticker.attach(&Blue_Toggle, 1.0);
            break;
        case 2://Red LED Flashing @5Hz Blue LED off
            blue_led = 1;
            blue_ticker.detach();
            red_ticker.attach(&Red_Toggle, 0.2);
            spkr_ticker.attach(&Speaker_On, 0.1);
            break;
        }
    }
float Calculate_Speaker_Frequency(){
    float fAverage = (MMA.z() + MMA.x() + MMA.y()) / 3;
    fAverage *= sensor.read();

    fAverage *= VOLUME;

    fAverage *= tempPot;

    return fAverage;
    }

void Speaker_On(){
    spkr.period(1.0 / Calculate_Speaker_Frequency()); //TODO PUT IN AVERAGE

    if(spkr != 0.5)
        spkr = 0.5;

}

void Red_Toggle(){
    red_led = !red_led;

}
void Blue_Toggle(){
    blue_led = !blue_led;

}