/*
  Copyright (c) 2011-2012 Arduino.  All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  See the GNU Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifdef TARGET_LPC1768

#include "../../inc/MarlinConfig.h"
#include "arduino.h"
#include "pinmapping.h"
//#include "HAL_timers.h"
#include "fastio.h"

#define GNUM 31

typedef void (*interruptCB)(void);

static interruptCB callbacksP0[GNUM];
static interruptCB callbacksP2[GNUM];

extern "C" void GpioEnableInt(const uint32_t port, const uint32_t pin, const uint32_t mode);
extern "C" void GpioDisableInt(const uint32_t port, const uint32_t pin);

//void deadloop(void) {}

/* Configure PIO interrupt sources */
static void __initialize() {
  for (uint8_t i = 0; i < GNUM; i++) {
    callbacksP0[i] = 0;
    callbacksP2[i] = 0;
  }
  NVIC_EnableIRQ(EINT3_IRQn);
}

void attachInterrupt(const uint32_t pin, void (*callback)(void), uint32_t mode) {
  static int enabled = 0;

  if (!INTERRUPT_PIN(pin)) return;

  if (!enabled) {
    __initialize();
    ++enabled;
  }
  uint8_t myport = LPC1768_PIN_PORT(pin),
          mypin = LPC1768_PIN_PIN(pin);

  if (myport == 0)
    callbacksP0[mypin] = callback;
  else
    callbacksP2[mypin] = callback;

  // Enable interrupt
  GpioEnableInt(myport,mypin,mode);
}

void detachInterrupt(const uint32_t pin) {
  if (!INTERRUPT_PIN(pin)) return;

  const uint8_t myport = LPC1768_PIN_PORT(pin),
                mypin = LPC1768_PIN_PIN(pin);

  // Disable interrupt
  GpioDisableInt(myport, mypin);

  // unset callback
  if (myport == 0)
    callbacksP0[mypin] = 0;
  else //if (myport == 2 )
    callbacksP2[mypin] = 0;
}


extern "C" void GpioEnableInt(uint32_t port, uint32_t pin, uint32_t mode) {
  //pin here is the processor pin, not logical pin
  if (port == 0) {
    LPC_GPIOINT->IO0IntClr = _BV(pin);
    if (mode == RISING) {
      SBI(LPC_GPIOINT->IO0IntEnR, pin);
      CBI(LPC_GPIOINT->IO0IntEnF, pin);
    }
    else if (mode == FALLING) {
      SBI(LPC_GPIOINT->IO0IntEnF, pin);
      CBI(LPC_GPIOINT->IO0IntEnR, pin);
    }
    else if (mode == CHANGE) {
      SBI(LPC_GPIOINT->IO0IntEnR, pin);
      SBI(LPC_GPIOINT->IO0IntEnF, pin);
    }
  }
  else {
    LPC_GPIOINT->IO2IntClr = _BV(pin);
    if (mode == RISING) {
      SBI(LPC_GPIOINT->IO2IntEnR, pin);
      CBI(LPC_GPIOINT->IO2IntEnF, pin);
    }
    else if (mode == FALLING) {
      SBI(LPC_GPIOINT->IO2IntEnF, pin);
      CBI(LPC_GPIOINT->IO2IntEnR, pin);
    }
    else if (mode == CHANGE) {
      SBI(LPC_GPIOINT->IO2IntEnR, pin);
      SBI(LPC_GPIOINT->IO2IntEnF, pin);
    }
  }
}

extern "C" void GpioDisableInt(const uint32_t port, const uint32_t pin) {
  if (port == 0) {
    CBI(LPC_GPIOINT->IO0IntEnR, pin);
    CBI(LPC_GPIOINT->IO0IntEnF, pin);
    LPC_GPIOINT->IO0IntClr = _BV(pin);
  }
  else {
    CBI(LPC_GPIOINT->IO2IntEnR, pin);
    CBI(LPC_GPIOINT->IO2IntEnF, pin);
    LPC_GPIOINT->IO2IntClr = _BV(pin);
  }
}

bool isPowerOf2(unsigned int n) {
  return n == 1 || (n & (n - 1)) == 0;
}

#if 0
  extern "C" void EINT3_IRQHandler () {
    LPC_GPIOINT->IO0IntClr = LPC_GPIOINT->IO2IntClr = 0xFFFFFFFF;
    TOGGLE(13);
    //NVIC_ClearPendingIRQ(EINT3_IRQn);
  }
#else

  extern "C" void EINT3_IRQHandler(void) {
    // Read in all current interrupt registers. We do this once as the
    // GPIO interrupt registers are on the APB bus, and this is slow.
    uint32_t rise0 = LPC_GPIOINT->IO0IntStatR,
             fall0 = LPC_GPIOINT->IO0IntStatF,
             rise2 = LPC_GPIOINT->IO2IntStatR,
             fall2 = LPC_GPIOINT->IO2IntStatF;
    // Clear the interrupts ASAP
    LPC_GPIOINT->IO0IntClr = LPC_GPIOINT->IO2IntClr = 0xFFFFFFFF;
    NVIC_ClearPendingIRQ(EINT3_IRQn);

    /* multiple pins changes happened.*/
    if (rise0) while (rise0 > 0) {      // Continue as long as there are interrupts pending
      const uint8_t bitloc = 31 - __CLZ(rise0); //CLZ returns number of leading zeros, 31 minus that is location of first pending interrupt
      if (callbacksP0[bitloc] != NULL) callbacksP0[bitloc]();
      rise0 -= _BV(bitloc);
    }

    if (fall0) while (fall0 > 0) {
      const uint8_t bitloc = 31 - __CLZ(fall0);
      if (callbacksP0[bitloc] != NULL) callbacksP0[bitloc]();
      fall0 -= _BV(bitloc);
    }

    if (rise2) while(rise2 > 0) {
      const uint8_t bitloc = 31 - __CLZ(rise2);
      if (callbacksP2[bitloc] != NULL) callbacksP2[bitloc]();
      //LPC_GPIOINT->IO2IntClr = 1 << bitloc;
      rise2 -= _BV(bitloc);
    }

    if (fall2) while (fall2 > 0) {
      const uint8_t bitloc = 31 - __CLZ(fall2);
      if (callbacksP2[bitloc] != NULL) callbacksP2[bitloc]();
      //LPC_GPIOINT->IO2IntClr = 1 << bitloc;
      fall2 -= _BV(bitloc);
    }
    //NVIC_ClearPendingIRQ(EINT3_IRQn);
    //LPC_GPIOINT->IO0IntClr = LPC_GPIOINT->IO2IntClr = 0xFFFFFFFF;
    //NVIC_ClearPendingIRQ(EINT3_IRQn);
  }

#endif

#endif // TARGET_LPC1768