// (C) 2020 Philip Endecott.
// Distributed under the Boost Software License, Version 1.0.
// See accompanying file LICENCE.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <array>
#include <cstdint>

#include "arm.hh"
#include "registers.hh"
#include "uart.hh"
#include "rtc.hh"
#include "adc.hh"
#include "cpu.hh"
#include "buffer.hh"
#include "timer.hh"
#include "wdt.hh"
#include "power.hh"


// Debug
// =====

// -DENABLE_DEBUG in the Makefile to turn this on.

#ifdef ENABLE_DEBUG
volatile Uart& debug_uart = uart0;
#endif

template <typename... T>
void debug(T&&... t [[maybe_unused]])
{
#ifdef ENABLE_DEBUG
  debug_uart.send(std::forward<T>(t)...);
#endif
}


// Modbus configuration:
// =====================

constexpr int baudrate = 9600;

enum adc_type_e { adc_raw, adc_voltage, adc_temperature };
struct adc_info { uint8_t port; adc_type_e type; };

struct output_info { uint8_t block; uint8_t bit; };

constexpr int timeout_s = 60;   // If no MODBUS commands have been successfully received and 
                                // replied to for this period, the device resets itself.

constexpr int max_modbus_frame_size = 16;
    // The protocol allows up to 256 bytes; the frames we understand 
    // are all shorter.  (Less than this in fact.)  If we receive a 
    // longer frame, we silently ignore the remaining bytes.

volatile Uart& modbus_uart = uart1;


#ifdef BOILER_CONTROLLER

constexpr int modbus_slave_address = 42;

constexpr int adc_channels = 2;
constexpr adc_info adc_infos[adc_channels] = { {1,adc_temperature}, {2,adc_temperature} };
// Note there is a problem with port 0 on the board in the boiler 
// controller; it reads as if it's shorted to power, or somethng.

constexpr int outputs = 1;
constexpr output_info output_infos[outputs] = { {0,16} };

#endif

#ifdef RAD_CONTROLLER

constexpr int modbus_slave_address = 43;

constexpr int adc_channels = 0;
constexpr adc_info adc_infos[adc_channels] = {};

constexpr int outputs = 20;
constexpr output_info output_infos[outputs] = {
  {1,16}, {1,17}, {1,18}, {1,19}, {1,20}, {1,21}, {1,22}, {1,23},
  {1,24}, {1,25}, {1,26}, {1,27}, {1,28}, {1,29}, {1,30}, {1,31},
  // The next pins along the edge of the board after P1.16-31 are P0.0-7.
  // P0.0 and P0.1 are the programming UART, so we don't use them; P0.2 and 
  // P0.3 are I2C pins that are open-drain only so we skip those; we use 
  // P0.4-7 (note the order).
  {0,6},  {0,7},  {0,4},  {0,5}
};

#endif


// ADC input:
// ==========

// We configure the RTC to generate an interrupt every second, and start 
// a conversion on the first channel.  When that completes and we get the 
// ADC interrupt we start a conversion on the next channel, etc.
// The read values are stored in adc_values[].
// (If we wanted more frequent conversions, or conversions between once 
// per second and once per minute, we could use the counter-timer rather 
// than the RTC.)

// Note the distinction between ports (i.e. pins) 0-3 and channels (i.e. 
// modbus register numbers) 0-adc_channels-1.
// Map from port to channel:
static constexpr int adc_port_to_channel(int port)
{
  for (int i = 0; i < adc_channels; ++i) {
    if (adc_infos[i].port == port) return i;
  }
  __builtin_unreachable();
}

__attribute__((interrupt("IRQ")))
__attribute__((target("arm")))
void rtc_irq_handler()
{
  *VICVectAddr = 1;
  *ILR = 3;  // Clear the interrupt(s).
  start_adc_read(adc_infos[0].port);
}

volatile int adc_values[adc_channels];

__attribute__((interrupt("IRQ")))
__attribute__((target("arm")))
void adc_irq_handler()
{
  *VICVectAddr = 1;
  auto r = *ADGDR;
  auto port = (r >> 24) & 7;         // Bits 26:24 are port that has completed.
  auto result  = (r & 0xffff) >> 6;  // Bits 15:6 are the result.
  auto channel = adc_port_to_channel(port);
  if (channel < adc_channels) adc_values[channel] = result;
  ++channel;
  if (channel < adc_channels) start_adc_read(adc_infos[channel].port);
}

static constexpr float v_supply = 3.3f;

static float tmp35_v_to_t(float v)
{
  return v / 0.010f;
}

static int get_adc_value(int channel)
{
  auto raw = adc_values[channel];
  auto type = adc_infos[channel].type;
  if (type == adc_raw) return raw;
  auto voltage = v_supply * raw / 1023.0f;
  if (type == adc_voltage) return 1000.0f * voltage;  // In mV
  if (type == adc_temperature) return 10.0f * tmp35_v_to_t(voltage);  // In 0.1 C
  __builtin_unreachable();
}


// GPIO outputs:
// =============

static void setup_gpio()
{
  // Make everything outputs.
  // Note this doesn't interfere with the pins that are configured to not 
  // be GPIOs, e.g. ADC inputs.
  *IO0DIR = 0xffffffff;  // 1 = output; default 0.
  *IO1DIR = 0xffffffff;  // 1 = output; default 0.
  *IO0PIN = 0;           // Initialise low.
  *IO1PIN = 0;           // Initialise low.
}

static void gpio0_set_bit(int bit)
{
  *IO0SET = 1<<bit;
}

static void gpio0_clear_bit(int bit)
{
  *IO0CLR = 1<<bit;
}

static void gpio1_set_bit(int bit)
{
  *IO1SET = 1<<bit;
}

static void gpio1_clear_bit(int bit)
{
  *IO1CLR = 1<<bit;
}

static void set_output(int output)
{
  debug("Set output ",output,'\n');
  auto info = output_infos[output-1];
  if (info.block == 0) gpio0_set_bit(info.bit);
  else                 gpio1_set_bit(info.bit);
}

static void clear_output(int output)
{
  debug("Clear output ",output,'\n');
  auto info = output_infos[output-1];
  if (info.block == 0) gpio0_clear_bit(info.bit);
  else                 gpio1_clear_bit(info.bit);
}


// Modbus command processing
// =========================

// Current receive frame:
volatile buffer<uint8_t,max_modbus_frame_size> rcv_buf;

// Modbus frames do not include a length field.  In many cases we can determine 
// the expected length by parsing the function code etc., but that is not 
// sufficient in general because other devices may use private function codes. 
// So we have to determine the end of the message with a timeout.  (This is in 
// fact what the standard requires.)
// Modbus requires a gap of at least 3.5 character times after each frame, and 
// prohibits gaps of more than 1.5 character times between each character.  So 
// if we wait for 3.5 character times with no further character received, we can 
// be sure that the frame has finished.

__attribute__((interrupt("IRQ")))
__attribute__((target("arm")))
void modbus_uart_irq_handler()
{
  *VICVectAddr = 1;
  modbus_uart.clear_interrupt();
  bool any_received = false;
  while (1) {
    auto s = modbus_uart.receive_status();
    switch (s) {
      case Uart::receive_status_e::error: {
        // Not tested!
        modbus_uart.receive_reset();
        rcv_buf.clear();
        break;
      }
      case Uart::receive_status_e::data: {
        auto c = modbus_uart.rcv();
        if (!rcv_buf.full()) rcv_buf.push_back(c);
        any_received = true;
        break;
      }
      case Uart::receive_status_e::idle: {
        goto break_outer;
      }
    }
  }
break_outer:
  if (any_received) {
    constexpr int uart_bit_period_us = 1'000'000 / baudrate;
    constexpr int timeout_us = uart_bit_period_us * 35;  // 3.5 character times of 10 bits per character.
    set_timer(timeout_us);
  }
}

volatile bool timer_expired;

__attribute__((interrupt("IRQ")))
__attribute__((target("arm")))
void timer0_irq_handler()
{
  *VICVectAddr = 1;
  *T0IR = 0xff;
  cancel_timer();
  timer_expired = true;
}

static void rcv_modbus_frame()
{
//  debug("Waiting for command...");
// Debug here causes a delay which may cause us to miss the start of the 
// next modbus command.
  rcv_buf.clear();
  timer_expired = false;
  // Need to wait until timer_expired has been set by the timer IRQ handler:
  //   while (!timer_expired) {}
  // (It's worth testing with that, as it might provoke volatile-related compiler 
  // optimisations that we need to guard against.)
  // That works but we would prefer to enter idle mode and use less power:
  //   while (!timer_expired) idle();
  // That doesn't work because the interrupt could occur between testing 
  // the flag and calling idle.  So we need to disable interrupts:
  while (!timer_expired) {
    disable_irq();
    if (!timer_expired) idle();
    enable_irq();
  }
  // This works because even DISabled interrupts wake up from idle mode.  
  // Note that it increases interrupt latency as the interrupt isn't taken 
  // until we re-enable it after idle() returns.  It's important that the 
  // dis/enable are inside the loop so that interrupts are briefly enabled 
  // after idle returns and the interrupt can actually be taken.
  // Question: does idle sleep immediately, or does pipelining cause some 
  // following instructions to execute before the clock stops?
  debug("received.\n");
}

template <typename ITER>
uint16_t crc(ITER begin, ITER end)
{
  uint16_t x = 0xffff;
  for (auto i = begin; i < end; ++i) {
    uint8_t c = *i;
    x = x ^ c;
    for (int j = 0; j < 8; ++j) {
      if (x & 1) { x = x >> 1; x = x ^ 0xa001; }
      else       { x = x >> 1; }
    }
  }
  return x;
}


template <typename T>
void modbus_send(const T& data)
{
  debug("Sending reply...");
  // We use RTS to enable the RS485 driver.
  // The driver chip has an active-high enable, but RTS is normally active low. 
  // As a hack we de-assert RTS to enable the driver.
  // We might want to add some padding time after asserting and before deasserting 
  // the enable.  (It looks OKish without it, though the deassert might be in the 
  // middle of the last stop bit.)
  modbus_uart.rts(false);
  modbus_uart.send(data);
  modbus_uart.wait_until_transmiter_empty();
  modbus_uart.rts(true);
  debug("done.\n");
}


static void reply_exception(uint8_t code)
{
  // Exception replies have the request's function code with its top bit 
  // inverted and an exception code in the next byte.
  debug("Exception reply ",static_cast<int>(code));
  buffer<uint8_t,5> response;
  response.push_back(modbus_slave_address);
  response.push_back(rcv_buf[1] | 0x80);
  response.push_back(code);
  auto c = crc(response.begin(),response.end());
  response.push_back(c & 0xff);
  response.push_back(c >> 8);
  modbus_send(response);
}

static void reply_ok_echo()
{
  debug("Success echo\n");
  // Successful completion of an operation with no result data, i.e. a 
  // write, is simply an echo of the received message.
  modbus_send(rcv_buf);
}

static bool validate_modbus_frame()   // Check if received frame is valid and addressed to us.
{
  auto size = rcv_buf.size();
  if (size < 4) return false;    // Can't possibly be valid; don't reply.

  auto address = rcv_buf[0];
  if (address != modbus_slave_address) return false;  // Not for us; don't reply.

  auto expected_crc = crc(&(rcv_buf[0]), &(rcv_buf[rcv_buf.size()-2]));
  auto received_crc = rcv_buf[rcv_buf.size()-2]
                    | (rcv_buf[rcv_buf.size()-1] << 8);
  if (received_crc != expected_crc) return false;  // Don't reply if CRC is wrong.  

  return true;
}

static void handle_modbus_read_holding_registers()
{
  if (rcv_buf.size() != 8) { reply_exception(3); return; }  // 3 = Illegal data value.
  uint16_t first  = (rcv_buf[2]<<8) | rcv_buf[3];
  uint16_t n_regs = (rcv_buf[4]<<8) | rcv_buf[5];
  auto last = first + n_regs;
  if (last > adc_channels) { reply_exception(2); return; }  // 2 = Illegal data address.
  buffer<uint8_t, 3 + adc_channels*2 + 2> response;
  response.push_back(modbus_slave_address);
  response.push_back(3);
  response.push_back(n_regs * 2);
  for (int r = first; r < last; ++r) {
    auto v = get_adc_value(r);
    response.push_back(v >> 8);
    response.push_back(v & 0xff);
  }
  auto c = crc(response.begin(),response.end());
  response.push_back(c & 0xff);
  response.push_back(c >> 8);
  modbus_send(response);
}

static void handle_modbus_write_single_register()
{
  if (rcv_buf.size() != 8) { reply_exception(3); return; }  // 3 = Illegal data value.
  uint16_t output = (rcv_buf[2]<<8) | rcv_buf[3];
  uint16_t cmd    = (rcv_buf[4]<<8) | rcv_buf[5];
  if (output < 1 || output > outputs) { reply_exception(2); return; }  // 2 = Illegal data address.
  if      (cmd == 0x0100) set_output(output);
  else if (cmd == 0x0200) clear_output(output);
  else { reply_exception(3); return; }  // 3 = Illegal data value.
  reply_ok_echo();
}

static void handle_modbus_frame()
{
  auto function = rcv_buf[1];
  switch (function) {
    case 3:  handle_modbus_read_holding_registers();
             break;
    case 6:  handle_modbus_write_single_register();
             break;
    default: reply_exception(1);  // 1 = Illegal function.
             break;
  }
}

static void modbus_loop()
{
  while (1) {
    rcv_modbus_frame();
    bool valid = validate_modbus_frame();
    if (valid) {
      handle_modbus_frame();
      wdt_atomic_poke();
    }
  }
}


// Interrupt Controller
// ====================

constexpr int vic_timer0_num = 4;
constexpr int vic_uart0_num  = 6;
constexpr int vic_uart1_num  = 7;
constexpr int vic_rtc_num    = 13;
constexpr int vic_adc_num    = 18;

__attribute__((interrupt("IRQ")))
__attribute__((target("arm")))
void default_irq_handler()
{
  debug("default irq!!\n");
  // Error.
  // Perhaps do more here if we want to ignore the interrupt, hmm.
}

static void setup_vic()
{
  *VICIntSelect   = 0;  // No FIQs.

  int slot = 0;
  uint32_t enables = 0;

  // I've not made this work using a function; I think the attributes on the 
  // handler functions cause trouble.
#define ADD_HANDLER(HANDLER, INTNUM)        \
  VICvectCntl[slot]  = INTNUM | (1<<5);     \
  VICVectAddrs[slot] = (uint32_t) &HANDLER; \
  enables |= 1<<INTNUM;                     \
  ++slot;

  // Put the highest priority interrupts first.
  // (Note that higher-priority interrupts don't interrupt lower priority ones, 
  // so the ordering here doesn't matter very much.(
  ADD_HANDLER(modbus_uart_irq_handler, vic_uart1_num);
  ADD_HANDLER(timer0_irq_handler,      vic_timer0_num);
  if constexpr (adc_channels) {
    ADD_HANDLER(adc_irq_handler,       vic_adc_num);
    ADD_HANDLER(rtc_irq_handler,       vic_rtc_num);
  }

#undef ADD_HANDLER

  *VICIntEnable = enables;
  // Note that writing to this register apparently sets bits but 
  // doesn't clear them; to clear bits, write ones to IntEnClear.
}


// Misc setup
// ==========

static void setup_power()
{
  set_peripheral_power( (adc_channels ? (PCAD | PCRTC) : 0)
#ifdef ENABLE_DEBUG
                      | PCUART0
#endif
                      | PCUART1 | PCTIM0
                      );
}

static void setup_pins()
{
  // Pin configuration.
  // Pins default to GPIOs.
  *PINSEL0 = (0b01 <<  0)  // P0.0 is UART0 TXD
           | (0b01 <<  2)  // P0.1 is UART0 RXD
           | (0b01 << 16)  // P0.8 is UART1 TXD
           | (0b01 << 18)  // P0.9 is UART1 RXD
           | (0b01 << 20)  // P0.10 is UART1 RTS
           ;
  *PINSEL1 = (0b01 << 22)  // P0.27 is AIN0
           | (0b01 << 24)  // P0.28 is AIN1
           | (0b01 << 26)  // P0.29 is AIN2
           | (0b01 << 28)  // P0.30 is AIN3
           ;
  *PINSEL2 = 0;
}


// main()
// ======

__attribute__((target("arm")))    // Needs to be ARM because it inlines functions that must be ARM, 
                                  // i.e. enable_irq().
int main(int /*argc*/, char* /*argv*/[])
{
  bool was_wdt_reset [[maybe_unused]] = wdt_timedout();  // Set an LED?

  setup_memmap();
  setup_wdt<timeout_s>();
  setup_mam();
  setup_power();
#ifdef ENABLE_DEBUG
  debug_uart.setup<38400,false>();
#endif
  modbus_uart.setup<baudrate,true>();
  modbus_uart.rts(true);  // Do this before configuring the pins, so the RS485 driver does not 
                          // glitch on during startup.
  setup_pins();
  setup_gpio();
  setup_timer();

  if constexpr (adc_channels) {
    setup_rtc();
    setup_adc();
  }

  setup_vic();

  if constexpr (adc_channels) {
    *CIIR = 1;  // Set RTC's IMSEC bit, interrupt every second.
  }

  debug("Hello world\n");
  if (was_wdt_reset) debug("Reset was due to watchdog\n");

  enable_irq();
  debug("Interrupts enabled\n");

  modbus_loop();

  while (1) {}
}