#include <bit>

#include "maple/maple.hpp"
#include "maple/maple_host_command_writer.hpp"
#include "maple/maple_bus_bits.hpp"
#include "maple/maple_bus_commands.hpp"
#include "maple/maple_bus_ft0.hpp"
#include "maple/maple_bus_ft9.hpp"

#include "sh7091/serial.hpp"

#include "math/math.hpp"

#include "input.hpp"
#include "cursor.hpp"
#include "framebuffer.hpp"

namespace cursor {

  struct cursor state[4];

  void init()
  {
    for (int port_ix = 0; port_ix < 4; port_ix++) {
      state[port_ix].x = framebuffer.px_width / 2;
      state[port_ix].y = framebuffer.px_height / 2;
    }
  }

  void update()
  {
    for (int port_ix = 0; port_ix < 4; port_ix++) {
      state[port_ix].active = false;

      auto& port = input::state.port[port_ix];
      if ((port.function_type & function_type::controller) != 0 && port.host_response_data_transfer_ft0 != nullptr) {
        auto& bus_data = port.host_response_data_transfer_ft0->bus_data;
        auto& data_fields = bus_data.data_fields;
        if ((std::byteswap(data_fields.function_type) & function_type::controller) == 0)
          continue;

        auto& data = data_fields.data;

        float dx = static_cast<float>(data.analog_coordinate_axis[2] - 0x80) * 0.015;
        float dy = static_cast<float>(data.analog_coordinate_axis[3] - 0x80) * 0.015;
        state[port_ix].x += dx;
        state[port_ix].y += dy;

	state[port_ix].a = ft0::data_transfer::digital_button::a(data.digital_button) == 0;
	state[port_ix].b = ft0::data_transfer::digital_button::b(data.digital_button) == 0;

        state[port_ix].active = true;
      } else if ((port.function_type & function_type::pointing) != 0 && port.host_response_data_transfer_ft9 != nullptr) {
        auto& bus_data = port.host_response_data_transfer_ft9->bus_data;
        auto& data_fields = bus_data.data_fields;
        if ((std::byteswap(data_fields.function_type) & function_type::pointing) == 0)
          continue;
        auto& data = data_fields.data;
        float dx = static_cast<float>(data.analog_coordinate_axis[0] - 0x200) * 0.65;
        float dy = static_cast<float>(data.analog_coordinate_axis[1] - 0x200) * 0.65;
        state[port_ix].x += dx;
        state[port_ix].y += dy;
        state[port_ix].a = ft9::data_transfer::digital_button::a(data.digital_button) == 0;
	state[port_ix].b = ft9::data_transfer::digital_button::b(data.digital_button) == 0;

        state[port_ix].active = true;
      }

      state[port_ix].x = clamp<float>(state[port_ix].x, 0, framebuffer.px_width - 1);
      state[port_ix].y = clamp<float>(state[port_ix].y, 0, framebuffer.px_height - 1);
    }
  }

}