using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Net;
using System.Net.Sockets;
using System.IO;
using System.Text;
using System.Linq;

public class ExperimentSfM : MonoBehaviour
{
    public float scale_factor = 1.0f;

    public Transform endoscopic_transform;
    public Transform light_transform;

    static UdpClient udp;
    private Vector3 camera_init_pos;
    private StreamWriter sw;


    // Use this for initialization
    void Start()
    {
        sw = new StreamWriter(@"SaveData.csv", true, Encoding.GetEncoding("Shift_JIS"));
        int LOCAL_PORT = 50007;

        udp = new UdpClient(LOCAL_PORT);
        udp.Client.ReceiveTimeout = 1000;
        camera_init_pos = endoscopic_transform.position;
    }

    // Update is called once per frame
    void Update()
    {

        IPEndPoint remoteEP = null;

        try
        {
            byte[] data = udp.Receive(ref remoteEP);
            string text = Encoding.UTF8.GetString(data);
            Debug.Log(text);
            string[] pose_arr = text.Split(',');
            List<float> pose_list = pose_arr.Select(x => float.Parse(x)).ToList();

            endoscopic_transform.Translate(new Vector3(scale_factor * pose_list[0], scale_factor * pose_list[1], scale_factor * pose_list[2]));
            endoscopic_transform.Rotate(new Vector3(pose_list[3], pose_list[4], pose_list[5]));
            light_transform.position = endoscopic_transform.position;
            var cur_position = endoscopic_transform.position - camera_init_pos;
            sw.WriteLine(string.Format("{0},{1},{2}", cur_position.x, cur_position.y, cur_position.z));
        }
        catch
        {

        }

    }

    void OnApplicationQuit()
    {
        sw.Close();    
    }
}