#include <iostream>
#include <iomanip>
#include <stdexcept>
#include <vector>
#include <boost/program_options.hpp>
#include <cassert>

#include "bit_manipulation.hpp"
#include "utils.hpp"
#include "cl_options.hpp"
#include "gbt_sca_i2c_adc.hpp"
#include "sampa.hpp"

using namespace gbt;
namespace bpo = boost::program_options;

/** Class wrapping relevant functionality of the SAMPA tester */
class SampaTester : public common::BitManipulation
{
  private:
    uint8_t verbosity_;

    /** Some SampaTester-related constants/bits */
    const uint8_t idx_sca_i2c_sampa_ = {0};
    const uint8_t idx_sca_i2c_ext_adc_ = {5};

  public:
    /** ADC inputs and mappings */
    using adc_input_t = std::tuple<std::string /*name*/, std::string /*unit*/, uint8_t /*adc_input*/, float /*conv_factor*/>;
    static const std::array<adc_input_t, 11> adc_inputs_;

    /** DAC output and mappings */
    using dac_output_t = std::tuple<std::string /*name*/, uint8_t /*dac_output*/>;
    static const std::array<dac_output_t, 4> dac_outputs_;

  protected:
    std::unique_ptr<gbt::ScaBasic> sca_basic_;
    std::unique_ptr<gbt::ScaId> sca_id_;
    std::unique_ptr<gbt::ScaGpio> sca_gpio_;
    std::unique_ptr<gbt::ScaDac> sca_dac_;
    std::unique_ptr<gbt::ScaI2cMax1161x> sca_ext_adc_;
    std::unique_ptr<gbt::ScaI2cSampa> sca_i2c_sampa_;

  public:
    SampaTester(SampaTester const &) = delete;
    SampaTester &operator=(SampaTester const &) = delete;

    /** Construct */
    SampaTester(common::HdlcCore& hdlc, uint8_t verbosity = 0) :
        verbosity_(verbosity),
        sca_basic_(new gbt::ScaBasic(hdlc)),
        sca_id_(new gbt::ScaId(hdlc)),
        sca_gpio_(new gbt::ScaGpio(hdlc)),
        sca_dac_(new gbt::ScaDac(hdlc)),
        sca_ext_adc_(new gbt::ScaI2cMax1161x(hdlc, idx_sca_i2c_ext_adc_)),
        sca_i2c_sampa_(new gbt::ScaI2cSampa(hdlc, idx_sca_i2c_sampa_))
      {
        sca_gpio_->setThrowOnScaReplyError(true);
        sca_dac_->setThrowOnScaReplyError(true);
        sca_ext_adc_->setThrowOnScaReplyError(true);

        // Auto-update I2C slave address to current value of hadd
        sca_i2c_sampa_->setSlaveAddress(getHadd());
      }

    /** Reference to GPIO port */
    gbt::ScaGpio& getScaGpio() const { return *(sca_gpio_.get()); }

    /** Reference to DAC port */
    gbt::ScaDac& getScaDac() const { return *(sca_dac_.get()); }

    /** Reference to SAMPA I2C instance */
    gbt::ScaI2c& getScaI2cSampa() const { return *(sca_i2c_sampa_.get()); }

    /** Reference to external ADC instance (connected via SCA I2C) */
    gbt::ScaI2cMax1161x& getScaExtAdc() const { return *(sca_ext_adc_.get()); }

    /** Get SAMPA power status
     *  @return 1 if on, 0 if off
     */
    inline uint8_t getSampaPower() const { return getBit(sca_gpio_->getDataOut(), 30); }

    /** Set SAMPA power status
     *  @param state Desired power state
     *  @return Read-back power state
     */
    inline uint8_t setSampaPower(uint8_t state) const {
        sca_gpio_->setDataOut(setBit(sca_gpio_->getDataOut(), 30, state));
        return getSampaPower();
      }

    /** Get GPIO MuxSel bit status  */
    inline uint8_t getMuxSel() const { return getBit(sca_gpio_->getDataOut(), 11); }

    /** Set GPIO MuxSel bit status
     *  @param val New value
     *  @return Read-back MuxSel bit status
     */
    inline uint8_t setMuxSel(uint8_t val) const {
        sca_gpio_->setDataOut(setBit(sca_gpio_->getDataOut(), 11, val));
        return getMuxSel();
      }

    /** Get GPIO clk_config
     *  @return Read-back clk_config
     */
    inline uint8_t getClkConfig() const { return getField(sca_gpio_->getDataOut(), 4, 7); }

    /** Set GPIO clk_config
     *  @param val New clk_config value
     *  @return Read-back clk_config
     */
    inline uint8_t setClkConfig(uint8_t val) const {
        sca_gpio_->setDataOut(setField(sca_gpio_->getDataOut(), 4, 7, val));
        return getClkConfig();
      }

    /** Get GPIO hardware address
     *  @return Read-back hardware address
     */
    inline uint8_t getHadd() const { return getField(sca_gpio_->getDataOut(), 0, 4); }

    /** Set GPIO hardware address
     *  @param New hadd value
     *  @return Read-back hadd
     */
    inline uint8_t setHadd(uint8_t val) const {
        sca_gpio_->setDataOut(setField(sca_gpio_->getDataOut(), 0, 4, val));
        return getHadd();
      }

    /** Sample ADC inputs specified by mask */
    void sampleExtAdc(uint32_t input_mask = 0xfff, bool json = false, std::ostream& os = std::cout) const {
      bool first(true);
      os << (json ? "{\n" : "");

      for (auto i: adc_inputs_) {
        if (input_mask & (1 << std::get<2>(i))) {
          auto adc_raw = sca_ext_adc_->sample(std::get<2>(i));

          os << (first ? "" : (json ? ",\n" : "\n"))
             << (json ? "  \"" : "[Ext ADC] ") << std::right << std::setw(json ? 0 : 8) << std::get<0>(i) << (json ? "\": {" : " : ")
             << (json ? "\"value\": " : "") << std::setw(6) << std::fixed << std::setprecision(6)
             << std::left << adc_raw*std::get<3>(i) << (json ? ", " : "")
             << (json ? "\"unit\": \"" : " " ) << std::right << std::get<1>(i) << (json ? "\"}": "");

          first = false;
        }
      }

      os << (json ? "\n}" : "") << "\n";
    }

    /** Get information about input to external ADC
     *  @param channel External ADC input
     *  @return adc_input_t tuple with name, channel number, conversion factor, etc
     */
    const adc_input_t& getExtAdcInputDetails(uint8_t channel) const {
      assert(channel < adc_inputs_.size());

      unsigned int i(0);
      for (i = 0; i < adc_inputs_.size(); i++)
        if (std::get<2>(adc_inputs_[i]) == channel)
          break;

      return adc_inputs_[i];
    }
};

// SampaTester ADC mappings/conversions
const std::array<SampaTester::adc_input_t, 11> SampaTester::adc_inputs_ = {
    std::make_tuple("VDDA",     "V",   0, 1./4096*2.048),
    std::make_tuple("VREF",     "V",   5, 1./4096*2.048),
    std::make_tuple("VDD ",     "V",   6, 1./4096*2.048),
    std::make_tuple("V450",     "V",   9, 1./4096*2.048),
    std::make_tuple("V600",     "V",   8, 1./4096*2.048),
    std::make_tuple("V750",     "V",  10, 1./4096*2.048),
    std::make_tuple("FE1_CURR", "V",  1, 1./4096*2.048),
    std::make_tuple("FE2_CURR", "V",  3, 1./4096*2.048),   // Wrong input in schematic: 7
    std::make_tuple("AD_CURR",  "V",  2, 1./4096*2.048),
    std::make_tuple("DR_CURR",  "V",  7, 1./4096*2.048),   // Wrong input in schematic: 3
    std::make_tuple("DG_CURR",  "V",  4, 1./4096*2.048)
    /* Measurements from Anders:
    Current in mA is 1000*(y-m)/50/0.1 = 200*(y-m), where y is in V; m is different for different stations
    => we do the conversion to mA "offline", from the logs
    */
  };


// SampaTester DAC mappings
const std::array<SampaTester::dac_output_t, 4> SampaTester::dac_outputs_ = {
    std::make_tuple("CG0", 0),
    std::make_tuple("POL", 1),
    std::make_tuple("CTS", 2),
    std::make_tuple("CG1", 3)
  };

/** dump SAMPA Global registers
 *  @return 0
 */
uint32_t dump_sampa_reg(const SampaTester& st)
{
  Sampa sampa(st.getScaI2cSampa());
  uint8_t clkConf = sampa.readRegister(Sampa::REG_CLKCONF);
  //N.B: the required clock-config change may not always have time to be set 
  //Better to do it via run.py control
  sampa.writeRegister(Sampa::REG_CLKCONF, 0x13); // for RingOscTest
  sampa.writeRegister(Sampa::REG_CMD, 0x7); // issue RingOscTest
  uint8_t ringCnt = (int) sampa.readRegister(Sampa::REG_RINGCNT);
  int clkSpeed = ringCnt - 0x7F;
  printf("ringCnt %d clkSpeed %d\n", ringCnt, clkSpeed);
  sampa.dumpGlobalRegisters();
  printf("Clock speed: %.1f MHz\n",
	 ((1 / (255 * 100e-9 / ((255 - clkSpeed) * 16))) / 1e6));

  sampa.writeRegister(Sampa::REG_CLKCONF, clkConf); // back to previous setting
  return 0;
}

//------------------------------------------------------------------------------
// Tests
//------------------------------------------------------------------------------

/** SAMPA I2C read test
 *  @return Number of read-errors encountered
 */
uint32_t test_sampa_i2c(const SampaTester& st, uint32_t iterations = 1000)
{
  Sampa sampa(st.getScaI2cSampa());
  uint32_t i2c_read_errors(0);
  uint32_t i2c_read_transactions(0);

  for (uint32_t i = 0; i < iterations; i++) {
    for (auto regs: Sampa::registers_) { // loop over (some - see sampa.hpp) SAMPA global regs
      try {
        i2c_read_transactions++;
        sampa.readRegister(regs.first);
      }
      catch (gbt::ScaException& e) {
        i2c_read_errors++;
      }
    }
  }

  // FIXME: make report a bit nicer
  std::cout << "i2c_read_transactions: " << i2c_read_transactions << "\n"
            << "i2c_read_errors: " << i2c_read_errors << "\n";

  return i2c_read_errors;
}


/** SAMPA ADCTRIM/voltage scan
 */
uint32_t test_adctrim_scan(const SampaTester& st, bool json = false, std::ostream& os = std::cout)
{
  Sampa sampa(st.getScaI2cSampa());
  const uint8_t adctrim_values(8);
  std::array<uint8_t, 3> inputs = {8, 9, 10};    // V450 .. V750
  std::array<float, inputs.size()*adctrim_values> meas;

  // Vary ADCTRIM and scan voltages
  for (uint8_t v = 0; v < adctrim_values; v++) {  // loop over ADCTRIM values
    sampa.writeRegister(Sampa::REG_ADCTRIM, v);

    for (uint8_t i = 0; i < inputs.size(); i++) {
      const SampaTester::adc_input_t& ai = st.getExtAdcInputDetails(inputs[i]);
      uint16_t adc_raw = st.getScaExtAdc().sample(inputs[i]);
//      std::cout << static_cast<uint32_t>(i) << "->" << static_cast<uint32_t>(adc_raw) << std::endl;
      meas[v*3+i] = adc_raw*std::get<3>(ai);
    }
  }

  // Print output (plain text or JSON version)
  os << (json ? "{\n" : "");

  // Print ADCTRIM line
  os << (json ? "\"" : "") << "ADCTRIM" << (json ? "\": [" : ": ");
  for (uint32_t i = 0; i < adctrim_values; i++)
    os << i << (i != (adctrim_values - 1) ? ", " : "");
  os << (json ? "]," : "") << "\n";

  // Print lines with measured values
  for (uint32_t i = 0; i < meas.size(); i++) {

    if (i == (i % inputs.size())) {
      const SampaTester::adc_input_t& ai = st.getExtAdcInputDetails(inputs[i]);
      os << (json ? "\"" : "") << std::get<0>(ai)
         << (json ? ("\": {\"unit\": \"" + std::get<1>(ai) + "\", [") : ("[" + std::get<1>(ai) + "]: "));
    }

    os << meas[i];

    if (i >= (meas.size()-inputs.size())) {
      os << (json ? "]}" : "") << ((json && (i != (meas.size() - 1))) ? "," : "") << "\n";
      if (i != (meas.size() - 1))
        i = i % inputs.size();
    }
    else {
      os << ", ";
      i += 2;
    }
  }
  os << (json ? "}\n" : "");

  return 0;
}


//------------------------------------------------------------------------------
// Command-line handling
//------------------------------------------------------------------------------
void control_dac_output(const bpo::variables_map& vm, SampaTester& st)
{
  for (auto op: SampaTester::dac_outputs_) {
    std::string cmd = std::get<0>(op);
    std::transform(cmd.begin(), cmd.end(), cmd.begin(), ::tolower);

    if (vm.count(cmd)) {
      const std::vector<cl_uint_t>& values = vm[cmd].as<std::vector<cl_uint_t>>();

      if (values.size() == 0) {
        // No value args, only get current DAC value and print
        uint8_t v = st.getScaDac().get(std::get<1>(op));
        std::cout << "[" << std::get<0>(op) << "] " << (v/256.) << "V (0x"
                  << std::hex << static_cast<uint32_t>(v) << std::dec << ")\n";
      }
      else {
        // New value(s) given, apply and read back updated DAC value
        for (auto v: values) {
          uint8_t nv = st.getScaDac().set(std::get<1>(op), v() & 0xff);
          std::cout << "[" << std::get<0>(op) << "] " << (nv/256.) << "V (0x"
                    << std::hex << static_cast<uint32_t>(nv) << std::dec << ")\n";
        }
      }
    }
  }
}


//------------------------------------------------------------------------------
void control_gpio(const bpo::variables_map& vm, SampaTester& st)
{
  using gpio_control_t = std::tuple<std::string /*name*/, std::function<uint32_t(uint32_t)> /*set*/, std::function<uint32_t(void)> /*get*/>;
  const gpio_control_t gpio_cmds[] = {
    std::make_tuple("sampa-pwr",  [&](uint32_t v){ return st.setSampaPower(v); }, [&](){ return st.getSampaPower(); }),
    std::make_tuple("mux-sel",    [&](uint32_t v){ return st.setMuxSel(v); },     [&](){ return st.getMuxSel(); }),
    std::make_tuple("hadd",       [&](uint32_t v){ return st.setHadd(v); },       [&](){ return st.getHadd(); }),
    std::make_tuple("clk-config", [&](uint32_t v){ return st.setClkConfig(v); },  [&](){ return st.getClkConfig(); })
  };

  for (auto gc: gpio_cmds) {
    const std::string& cmd = std::get<0>(gc);
    if (vm.count(cmd)) {
      const std::vector<cl_uint_t>& values = vm[cmd].as<std::vector<cl_uint_t>>();

      if (values.size() == 0) {
        // No value args, only get value
        std::cout << "[" << cmd << "] 0x" << std::hex << std::get<2>(gc)() << std::dec << std::endl;
      }
      else {
        // Value args given, apply in order
        for (auto v: values) {
          std::cout << "[" << cmd << "] 0x" << std::hex << std::get<1>(gc)(v()) << std::dec << std::endl;
        }
      }
    }
  }
}


//------------------------------------------------------------------------------
// Main
//------------------------------------------------------------------------------
int main(int argc, char** argv)
{
  // Short tool description
  const std::string tool_help = \
    "Usage:\n  " + std::string(argv[0]) + " <cmds/options>\n"\
    "  Tool will apply the operations on all SCAs defined by the SCA mask\n"\
    "Commands / Options";

  // clo will already have the default options added
  common::CommandLineOptions clo(tool_help);

  // Add custom options for this tool
  clo.addCmdLine("adc-all", "Sample and print all ext. ADC inputs")
     .addCmdLine("adc-port", bpo::value<cl_uint_t>()->implicit_value(0)->notifier([](cl_uint_t v) {
            if(v() >= SampaTester::adc_inputs_.size()) {
              throw bpo::validation_error(bpo::validation_error::invalid_option_value, "adc port"); } } ),
            "Initiate ext. ADC sampling cycle on port <n>")
     .addCmdLine("sampa-pwr", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given: 1 -> on, 0 -> off) or get SAMPA power status")
     .addCmdLine("hadd", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get hardward address (on GPIO)")
     .addCmdLine("clk-config", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get clock config (on GPIO)")
     .addCmdLine("mux-sel", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get mux select (on GPIO)")
     .addCmdLine("cg0", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get value for CG0 [0x0 .. 0xff]")
     .addCmdLine("cg1", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get value for CG1 [0x0 .. 0xff]")
     .addCmdLine("pol", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get value for POL [0x0 .. 0xff]")
     .addCmdLine("cts", bpo::value<std::vector<cl_uint_t>>()->multitoken()->zero_tokens(),
                 "Set (if arg given) or get value for CTS [0x0 .. 0xff")
     .addCmdLine("dump-sampa-reg", "Dump SAMPA global registers")
     .addCmdLine("test-sampa-i2c", "Run SAMPA I2C read test")
     .addCmdLine("test-adctrim-scan", "Vary ADCTRIM and scan voltages")
     .addCmdLine("json", "Output results as json (if applicable/supported)")
     ;

  // Process options
  auto vm = clo.Process(argc, argv);

  // Get bar
  std::unique_ptr<common::Bar> bar;
  try {
    bar.reset(common::BarFactory::makeBar(clo.getId(), clo.getBar()));
  } catch (std::exception& e) {
    std::cerr << e.what() << std::endl;
    exit(1);
  }

  // Do stuff
  try {
    // SCA loop
    for (uint32_t sca_idx = 0; sca_idx < 32; sca_idx++) {

      // Specified SCAs
      if ((clo.getFecMask() >> sca_idx) & 0x1) {

        if (clo.getVerboseFlag())
          std::cout << "Communicating with SCA" << sca_idx << std::endl;

        // HDLC core and Sampa tester instances for this SCA
        std::unique_ptr<common::HdlcCore>
          hdlc_core(common::HdlcFactory::makeHdlcCore(*(bar.get()), sca_idx, true));

        SampaTester st(*(hdlc_core.get()), vm.count("verbose") ? 1 : 0);

        // GPIO: SAMPA power, hardware address, clock config, mux sel -----
        if (vm.count("sampa-pwr") || vm.count("hadd") || vm.count("clk-config") || vm.count("mux-sel"))
          control_gpio(vm, st);

        // DAC ------------------------------------------------------------
        if (vm.count("cg1") || vm.count("cg0") || vm.count("cts") || vm.count("pol"))
          control_dac_output(vm, st);

        // ADC ------------------------------------------------------------
        if (vm.count("adc-port"))
          st.sampleExtAdc(1 << vm["adc-port"].as<cl_uint_t>()(), vm.count("json") != 0);

        if (vm.count("adc-all"))
          st.sampleExtAdc(0xfff, vm.count("json") != 0);

        if (vm.count("dump-sampa-reg"))
          dump_sampa_reg(st);

        // Tests ----------------------------------------------------------
        if (vm.count("test-sampa-i2c"))
          test_sampa_i2c(st);

        if (vm.count("test-adctrim-scan"))
          test_adctrim_scan(st, vm.count("json") != 0);

      }   // Specified SCAs
    }   // SCA loop
  }
  catch (ScaException& e) {
    std::cerr << "### SCA error ###\n" << e.what() << std::endl;
    exit(101);
  }
  catch (std::exception& e) {
    std::cerr << "### General error ###\n" << e.what() << ", exiting" << std::endl;
    exit(100);
  }

  return 0;
}