<template>
  <v-navigation-drawer v-model="store.rosBarOpened" temporary location="right">
    <v-container fluid>
      <v-row justify="space-between">
        <v-col cols="6" md="12">
          <v-text-field
            v-model="formState.ipaddress"
            label="IP Address"
            :disabled="connectionState"
          />
          <v-text-field
            v-model="formState.port"
            label="Port"
            :disabled="connectionState"
          />
          <v-switch
            @change="updateConnectionState"
            true-value="connected"
            false-value="not connected"
            :label="`${formState.connect}`"
          />
        </v-col>
      </v-row>
    </v-container>
  </v-navigation-drawer>
</template>

<script setup>
import * as ROS from "roslib";
import { useBWStore } from "@/store/BaseWebsiteStore";
import { usePStore } from "@/store/PolicyStore";
import { useCStore } from "@/store/ControlStore";

import { reactive, ref, watch } from "vue";

import * as ROSLIB from "roslib";
import { useRStore } from "@/store/RewardStore";
import { useMCStore } from "@/store/MountainCarStore";

const store = useBWStore();
const pstore = usePStore();
const cstore = useCStore();
const rstore = useRStore();
const mcstore = useMCStore();

const formState = reactive({
  ipaddress: "localhost",
  port: "9090",
  connect: "not connected",
  // connectionState: computed(() => formState.connect !== 'not connected'),
});

const connectionState = ref(false);
const stateCounter = ref([0, 0, 0]);

// Open Connection to ROSBridge and subscribe the topics.
const ros = new ROS.Ros();
const ros_eval = new ROS.Ros();

ros.on("connection", () => {
  stateCounter.value[0] += 1;
});
ros_eval.on("connection", () => {});

ros.on("error", (error) => {
  stateCounter.value[1] += 1;
  console.log(error);
});
ros_eval.on("error", () => {});

ros.on("close", () => {
  stateCounter.value[2] += 1;
});
ros_eval.on("close", () => {});

function updateConnectionState() {
  connectionState.value = !connectionState.value;
  if (formState.connect === "not connected") {
    formState.connect = "connected";
    const rosUrl = `ws://${formState.ipaddress}:${formState.port}`;
    ros.connect(rosUrl);
    ros_eval.connect(rosUrl);
  } else {
    formState.connect = "not connected";
    ros.close();
    ros_eval.close();
  }
}

const lastStateCounter = [0, 0, 0];

// ROS2 State Observer
watch(stateCounter.value, (newValue) => {
  let currentState = 0;
  for (let i = 0; i < lastStateCounter.length; i += 1) {
    if (newValue[i] - lastStateCounter[i] === 1) {
      currentState = i;
    }
    lastStateCounter[i] = newValue[i];
  }
  if (currentState === 0) {
    store.setRosConState("connected");
  } else if (currentState === 1) {
    store.setRosConState("error");
  } else if (currentState === 2) {
    store.setRosConState("not connected");
  }
});

// Policy Service
const policy_service = new ROSLIB.Service({
  ros: ros,
  name: "/policy_srv",
  serviceType: "active_bo_msgs/srv/WeightToPolicy",
});

// RL Service + Feedback Suscriber
const rl_feedback_subscriber = new ROSLIB.Topic({
  ros: ros,
  name: "/rl_feedback",
  messageType: "active_bo_msgs/msg/ImageFeedback",
});

rl_feedback_subscriber.subscribe((msg) => {
  mcstore.setDim(msg.height, msg.width);
  mcstore.setRgbArrays(msg.red, msg.green, msg.blue);
});

const active_rl_eval_sub = new ROSLIB.Topic({
  ros: ros_eval,
  name: "/active_rl_eval_request",
  messageType: "active_bo_msgs/msg/ActiveRLEval",
});

const pendingRequest = ref(false);
active_rl_eval_sub.subscribe((msg) => {
  pstore.setPolicy(msg.policy);
  pstore.setWeights(msg.weights);
  pendingRequest.value = true;
});

const active_rl_eval_pub = new ROSLIB.Topic({
  ros: ros_eval,
  name: "/active_rl_eval_response",
  messageType: "active_bo_msgs/msg/ActiveRLEval",
});

const active_eval_response_first = new ROSLIB.Message({
  policy: pstore.policy,
  weights: pstore.weights,
});
console.log(active_eval_response_first);
// active_rl_eval_pub.publish(active_eval_response_first);

// function sleep(ms) {
//   return new Promise((resolve) => setTimeout(resolve, ms));
// }

watch(
  () => cstore.getSendWeights,
  () => {
    const usr_mode = cstore.getUserMode;

    if (usr_mode === "manually") {
      const policy_request = new ROSLIB.ServiceRequest({
        weights: pstore.policy,
        nr_steps: pstore.weights,
      });

      policy_service.callService(policy_request, function (result) {
        pstore.setPolicy(result.policy);
      });
    } else if (usr_mode === "active BO") {
      if (!pendingRequest.value) {
        return;
      }
      console.log("Button pressed!");
      const active_eval_response = new ROSLIB.Message({
        policy: pstore.policy,
        weights: pstore.weights,
      });

      console.log(active_eval_response);

      active_rl_eval_pub.publish(active_eval_response);
      // await sleep(1000);
      console.log("New Policy/ Weights published");
      // pendingRequest.value = false;
    }
  }
);

const rl_service = new ROSLIB.Service({
  ros: ros,
  name: "/rl_srv",
  serviceType: "active_bo_msgs/srv/RLRollOut",
});

const bo_service = new ROSLIB.Service({
  ros: ros,
  name: "/bo_srv",
  serviceType: "active_bo_msgs/srv/BO",
});

const active_bo_service = new ROSLIB.Service({
  ros: ros,
  name: "/active_bo_srv",
  serviceType: "active_bo_msgs/srv/ActiveBO",
});

watch(
  () => cstore.getRunner,
  () => {
    const usr_mode = cstore.getUserMode;

    if (usr_mode === "manually") {
      const rl_request = new ROSLIB.ServiceRequest({
        policy: pstore.policy,
      });

      rl_service.callService(rl_request, function (rl_response) {
        rstore.addMeanManually(rl_response.reward);
      });
    } else if (usr_mode === "BO") {
      const bo_request = new ROSLIB.ServiceRequest({
        nr_weights: pstore.nr_weights,
        max_steps: pstore.max_steps,
        nr_episodes: cstore.nr_episodes,
        nr_runs: cstore.nr_runs,
        acquisition_function: cstore.acq_fun,
      });

      bo_service.callService(bo_request, function (bo_response) {
        pstore.setPolicy(bo_response.best_policy);
        pstore.setWeights(bo_response.best_weights);
        rstore.setMean(bo_response.reward_mean);
        rstore.setStd(bo_response.reward_std);
      });
      const rl_request = new ROSLIB.ServiceRequest({
        policy: pstore.policy,
      });

      rl_service.callService(rl_request, () => {});
    } else if (usr_mode === "active BO") {
      const active_bo_request = new ROSLIB.ServiceRequest({
        nr_weights: pstore.nr_weights,
        max_steps: pstore.max_steps,
        nr_episodes: cstore.nr_episodes,
        nr_runs: cstore.nr_runs,
        acquisition_function: cstore.acq_fun,
        epsilon: cstore.epsilon,
      });

      active_bo_service.callService(
        active_bo_request,
        function (active_bo_response) {
          pstore.setPolicy(active_bo_response.best_policy);
          pstore.setWeights(active_bo_response.best_weights);
          rstore.setMean(active_bo_response.reward_mean);
          rstore.setStd(active_bo_response.reward_std);
        }
      );
      const rl_request = new ROSLIB.ServiceRequest({
        policy: pstore.policy,
      });

      rl_service.callService(rl_request, () => {});
    }
  }
);
</script>

<style scoped></style>
