#include <Arduino.h>
#include <DFPlayer_Mini_Mp3.h>

//////////////////////////////////////////////  SET PINS   //////////////////////////
SoftwareSerial mySerial(10, 11); // RX, TX
#define txPinIR 3 //IR carrier output

#define IRpin_PIN      PINB
#define IRpin          1   // PIN D9 - atmega PB1


//#define TSOP 8
#define led_red A0
#define led_green A1
#define led_blue A2
//////////////////////////////////////////////  SET PINS   //////////////////////////

unsigned long duration = 0;
unsigned long durationold = 0;
long timeout = 10000; //
long Red_score = 0;
long Blue_score = 0;
long Yellow_score = 0;
long Green_score = 0;
long score = 0;
int Team = 5;
//unsigned long start_time;
String Received = "";
String Receivedold = "";
unsigned char carrierFreq = 56; //default
unsigned char period = 0;      //calculated once for each signal sent in initSoftPWM
unsigned char periodHigh = 0;  //calculated once for each signal sent in initSoftPWM
unsigned char periodLow = 0;   //calculated once for each signal sent in initSoftPWM
unsigned long sigTime = 0;     //used in mark & space functions to keep track of time
unsigned long sigStart = 0;    //used to calculate correct length of existing signal, to handle some repeats

#define NEC_HEX_VALUE 0x20DF22DDUL //UL makes this an unsigned long
#define NEC_BIT_COUNT 24           // header +24 for message
/////////////////////////////////////////////////////////////////////
// MilesTag command pack  (0x...  ...UL)
#define New_GameIM 0x8305E8UL // New Game (Immediate)
#define Explode 0x830BE8UL    //Explode
//////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////
const String New_GameIM_rcv = "100000110000010111101000";
const String Admin_Kill_rcv = "100000110000000011101000";
const String Clear_Scores_rcv = "100000110001010011101000";
///////////////////////////////////////////////////


uint8_t currentpulse = 0; // index for pulses we're storing
#define MAXPULSE 30
#define NUMPULSES 30

#define RESOLUTION 50
uint16_t pulses[NUMPULSES][2];  // pair is high and low pulse


class Elapsed
{
  unsigned long startus, startms;

public:
  // constructor resets time
  Elapsed()
  {
    reset();
  };

  // reset time to now
  void reset()
  {
    startus = micros();
    startms = millis();
  };

  // return Elapsed time in milliseconds
  unsigned long intervalMs()
  {
    return millis() - startms;
  };

  // return Elapsed time in microseconds
  unsigned long intervalUs()
  {
    return micros() - startus;
  };

}; // end of class Elapsed

static Elapsed t_1, t_2, t_3, t_bazook, t_flash, t_blink, t_itter;
int blink = 0;
int bit = 0;
int loopNr = 0;

void setup()
{
  Serial.begin(2000000); // 2MHz- fastest one
  Serial.println("---Ready----");
  Serial.println("\n");
  //pinMode(TSOP, INPUT);
  pinMode(txPinIR, OUTPUT);
  pinMode(led_red, OUTPUT);
  pinMode(led_green, OUTPUT);
  pinMode(led_blue, OUTPUT);
  digitalWrite(led_red, 1);
  digitalWrite(led_blue, 1);
  digitalWrite(led_green, 1);
  //white();

  delay(300);
  mySerial.begin(9600);
  mp3_set_serial(mySerial); //set softwareSerial for DFPlayer-mini mp3 module
  mp3_set_volume(30);
  mp3_play(10);
  delay(300);

  //led_off();

}

void initSoftPWM(unsigned char carrierFreq)
{ // Assumes standard 8-bit Arduino, running at 16Mhz
  //supported values are 30, 33, 36, 38, 40, 56 kHz, any other value defaults to 38kHz
  //we will aim for a  duty cycle of circa 33%

  period = (1000 + carrierFreq / 2) / carrierFreq;
  periodHigh = (period + 1) / 3; // 3
  periodLow = period - periodHigh;

  switch (carrierFreq)
  {
  case 30:           //delivers a carrier frequency of 29.8kHz & duty cycle of 34.52%
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope
    periodLow -= 10; //Trim it based on measurementt from Oscilloscope
    break;

  case 33:           //delivers a carrier frequency of 32.7kHz & duty cycle of 34.64%
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope
    periodLow -= 10; //Trim it based on measurementt from Oscilloscope
    break;

  case 36:           //delivers a carrier frequency of 36.2kHz & duty cycle of 35.14%
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope
    periodLow -= 11; //Trim it based on measurementt from Oscilloscope
    break;

  case 40:           //delivers a carrier frequency of 40.6kHz & duty cycle of 34.96%
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope
    periodLow -= 11; //Trim it based on measurementt from Oscilloscope
    break;

  case 56:           //delivers a carrier frequency of 53.8kHz & duty cycle of 40.86%
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope 6
    periodLow -= 12; //Trim it based on measurementt from Oscilloscope 12
    //Serial.println(periodHigh);
    //Serial.println(periodLow);

    break;

  case 38: //delivers a carrier frequency of 37.6kHz & duty cycle of 36.47%
  default:
    periodHigh -= 6; //Trim it based on measurementt from Oscilloscope
    periodLow -= 11; //Trim it based on measurementt from Oscilloscope
    break;
  }
}

void mark(unsigned int mLen)
{                  //uses sigTime as end parameter
  sigTime += mLen; //mark ends at new sigTime
  unsigned long now = micros();
  unsigned long dur = sigTime - now; //allows for rolling time adjustment due to code execution delays
  if (dur == 0)
    return;
  while ((micros() - now) < dur)
  { //just wait here until time is up
    digitalWrite(txPinIR, HIGH);
    if (periodHigh)
      delayMicroseconds(periodHigh);
    digitalWrite(txPinIR, LOW);
    if (periodLow)
      delayMicroseconds(periodLow);
  }
}

void space(unsigned int sLen)
{                  //uses sigTime as end parameter
  sigTime += sLen; //space ends at new sigTime
  unsigned long now = micros();
  unsigned long dur = sigTime - now; //allows for rolling time adjustment due to code execution delays
  if (dur == 0)
    return;
  while ((micros() - now) < dur)
    ; //just wait here until time is up
}

void sendHexNEC(unsigned long sigCode, byte numBits, unsigned char repeats, unsigned char kHz)
{

#define NEC_HEADER_MARK 2400
#define NEC_HEADER_SPACE 600
#define NEC_ONE_MARK 1200
#define NEC_ZERO_MARK 600
#define NEC_ONE_SPACE 600
#define NEC_ZERO_SPACE 600
#define NEC_TRAILER_MARK 560

  unsigned long bitMask = (unsigned long)1 << (numBits - 1); //allows for signal from 1 bit up to 32 bits
  //
  if (carrierFreq != kHz)
    initSoftPWM(kHz); //we only need to re-initialise if it has changed from last signal sent

  sigTime = micros(); //keeps rolling track of signal time to avoid impact of loop & code execution delays
  sigStart = sigTime; //remember for calculating first repeat gap (space), must end 108ms after signal starts

  // First send header Mark & Space
  mark(NEC_HEADER_MARK);
  space(NEC_HEADER_SPACE);

  while (bitMask)
  {
    if (bitMask & sigCode)
    { //its a One bit
      mark(NEC_ONE_MARK);
      space(NEC_ONE_SPACE);
    }
    else
    { // its a Zero bit
      mark(NEC_ZERO_MARK);
      space(NEC_ZERO_SPACE);
    }
    bitMask = (unsigned long)bitMask >> 1; // shift the mask bit along until it reaches zero & we exit the while loop
  }
  // Last send NEC Trailer MArk
  // mark(NEC_TRAILER_MARK);

  if (repeats == 0)
    return; //finished - no repeats
  else if (repeats > 0)
  {                                       //first repeat must start 108ms after first signal
    space(108000 - (sigTime - sigStart)); //first repeat Header should start 108ms after first signal
    mark(NEC_HEADER_MARK);
    space(NEC_HEADER_SPACE / 2); //half the length for repeats
    mark(NEC_TRAILER_MARK);
  }

  while (--repeats > 0)
  {                                                                            //now send any remaining repeats
    space(108000 - NEC_HEADER_MARK - NEC_HEADER_SPACE / 2 - NEC_TRAILER_MARK); //subsequent repeat Header must start 108ms after previous repeat signal
    mark(NEC_HEADER_MARK);
    space(NEC_HEADER_SPACE / 2); //half the length for repeats
    mark(NEC_TRAILER_MARK);
  }
}



void red()
{
  digitalWrite(led_red, 0);
  digitalWrite(led_blue, 1);
  digitalWrite(led_green, 1);
}
void blue()
{
  digitalWrite(led_red, 1);
  digitalWrite(led_blue, 0);
  digitalWrite(led_green, 1);
}
void green()
{
  digitalWrite(led_red, 1);
  digitalWrite(led_blue, 1);
  digitalWrite(led_green, 0);
}
void white()
{
  digitalWrite(led_red, 0);
  digitalWrite(led_blue, 0);
  digitalWrite(led_green, 0);
}
void yellow()
{
  digitalWrite(led_red, 0);
  digitalWrite(led_blue, 1);
  digitalWrite(led_green, 0);
}
void led_off()
{
  digitalWrite(led_red, 1);
  digitalWrite(led_blue, 1);
  digitalWrite(led_green, 1);
}

void reloading(){
for (int i = 0; i < 25; i++)
{
   white();
  delay(400);
  led_off();
  delay(600);
}
}

void score_clear()
{
  //Red_score = Blue_score = Yellow_score = Green_score = score = 0;
  Red_score = 0;
  Blue_score = 0;
  Yellow_score = 0;
  Green_score = 0;
  score = 0;
  //led_off();
  Serial.println("cleared scores");
}


void overscore()
{
  if (Red_score > timeout)
    {
      Serial.println("Red wins");
      Red_score = Blue_score = Yellow_score = Green_score = score = 0;

     }
  if (Blue_score > timeout)
    {
      Serial.println("Blue wins");
      Red_score = Blue_score = Yellow_score = Green_score = score = 0;
    }
  if (Yellow_score > timeout)
    {
      Serial.println("Yellow wins");
      Red_score = Blue_score = Yellow_score = Green_score = score = 0;
    }
  if (Green_score > timeout)
    {
      Serial.println("Green wins");
      Red_score = Blue_score = Yellow_score = Green_score = score = 0;
    }
  Serial.println("overscore");
}





void shootrocket()
{

  led_off();
  delay(100);
    for (int i = 0; i < 3; i++)
    {
      sendHexNEC(Explode, NEC_BIT_COUNT, 1, 56);
      delay(100);
    }
  delay(100);

  mp3_play(8);
  red();
  delay(3000);
  led_off();
  //reloading();
        for (int i = 0; i < 25; i++)
    {
      led_off();
      delay(600);
      blue();
      delay(400);

    }
  green();
}

void testowanie()
{
  Serial.println("test");
  for (int i = 0; i < 5000; i++)
  {
    blue();
    delay(200);
    led_off();
    delay(200);
    sendHexNEC(New_GameIM, NEC_BIT_COUNT, 1, 56);
    delay(100);
    red();
    delay(200);
    led_off();
    delay(200);
    sendHexNEC(New_GameIM, NEC_BIT_COUNT, 1, 56);
    delay(100);
    Serial.println("test");
  }
}

void update_team(){

 // duration = millis();

  //      if (durationold != 0){
   //       score = (duration - durationold);
    //      }

        //Serial.println(Received); // odebrany binarny

        int Team = (Received.substring(9,11).toInt());


        //Serial.println(Team);

            switch (Team)
                {
                case 0:
                  /* RED */
       //           Red_score += score;
                  Serial.println("RED");
                  red();
                  break;
                case 1:
                  /* BLUE */
        //          Blue_score += score;
                  //Serial.println("BLUE");
                  blue();
                  break;
                case 10:
                  /* YELLOW */
        //          Yellow_score += score;
                 // Serial.println("YELLOW");
                  yellow();
                  break;
                case 11:
                  /* GREEN */
         //         Green_score += score;
                  //Serial.println("GREEN");
                  green();
                  break;
                  case 5:
                  //itter start
                  break;


                default:
                //  Serial.println("default");
                  break;
                }

        durationold = duration;


  }

void update_time(){
 duration = millis();

        if (durationold != 0){
          score = (duration - durationold);
          }

        //Serial.println(Received); // odebrany binarny ;

        //Serial.println(Team);

            switch (Team)
                {
                case 0:
                  /* RED */
                  Red_score += score;
                  Serial.println("RED+1");
                  red();
                  break;
                case 1:
                  /* BLUE */
                  Blue_score += score;
                  //Serial.println("BLUE");
                  blue();
                  break;
                case 10:
                  /* YELLOW */
                  Yellow_score += score;
                 // Serial.println("YELLOW");
                  yellow();
                  break;
                case 11:
                  /* GREEN */
                  Green_score += score;
                  //Serial.println("GREEN");
                  green();
                  break;


                default:
                 // Serial.println(Team);
                  break;
                }

        durationold = duration;


}


void action(){

      if ((Received.length() == 15) && (Received.startsWith("20"))) //Shoot
      {

        update_team();




      }

    if ((Received.length() == 25) && (Received.startsWith("21"))) //Message
      {
        Serial.println("Message");
        testowanie();
        Serial.println(Received);
        mp3_play(10);
      }
}






void printpulses(void) {
  //Serial.println("\n\r\n\rReceived:");
      for (uint8_t i = 0; i < currentpulse; i++) {
        bit = (log(((((pulses[i][1]) * (RESOLUTION)) + 50) / 600)) / (log(2))) ;
        Received.concat(bit);
      }

}

int listenForIR(void) {
  currentpulse = 0;
  while (1) {
    uint16_t highpulse, lowpulse;  // temporary storage timing
    highpulse = lowpulse = 0; // start out with no pulse length

    while (IRpin_PIN & (1 << IRpin)) {
      highpulse++;
      //Serial.println(highpulse);

      if (highpulse >= 200)
      {



        return currentpulse;
      }

      delayMicroseconds(RESOLUTION);


      if (((highpulse >= MAXPULSE) && (currentpulse != 0)) || currentpulse == NUMPULSES) {
        //printpulses();   //RAW data
            for (uint8_t i = 0; i < currentpulse; i++) {
            bit = (log(((((pulses[i][1]) * (RESOLUTION)) + 50) / 600)) / (log(2))) ;
            Received.concat(bit);
          }

      Serial.println(Received);



        return currentpulse;
      }
    }
    pulses[currentpulse][0] = highpulse;


    while (! (IRpin_PIN & _BV(IRpin))) {
      lowpulse++;
      delayMicroseconds(RESOLUTION);

      if (((lowpulse >= MAXPULSE)  && (currentpulse != 0)) || currentpulse == NUMPULSES) {
        //printpulses(); //RAW data

            for (uint8_t i = 0; i < currentpulse; i++)
              {
              bit = (log(((((pulses[i][1]) * (RESOLUTION)) + 50) / 600)) / (log(2))) ;
              Received.concat(bit);
              }
            return currentpulse;
      }
    }
    pulses[currentpulse][1] = lowpulse;
    currentpulse++;
  }
}





void test_speaker()
{
  for (int i = 0; i < 9; i++)
  {

  mp3_play(i);
  delay(1000);
  }

}

void order()
{
  String commandline = Serial.readString();
  //Serial.println(commandline);

  if (commandline.startsWith("new game"))
  {
    Serial.println(F("Sending New Game (Immediate)"));
    sendHexNEC(New_GameIM, NEC_BIT_COUNT, 1, 56);
  }

  if (commandline.startsWith("shoot"))
  {
    shootrocket();
  }
  if (commandline.startsWith("test"))
  {
    testowanie();
  }
}

void reloadingold()
{

  if (blink == 2)
  {
    blink = 0;
  }

  if (t_blink.intervalMs() > 800)
  {
    if (blink == 0)
    {
      blue();
    }
    if (blink == 1)
    {
      led_off();
    }
    blink++;
    t_blink.reset();
    t_itter.reset();
  }
}

void loop()
{

 // testowanie();

      Received = "";
      listenForIR();

      if(Received != Receivedold){

        action();

        Receivedold = Received;
      }


      //update_team();
      update_time();




















      if (Received != "")
      {
        action();
        /* code */
      }


  //update();

  // action();
  // overscore();







}