-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)
function rotate_point(pt,angle)
    return({math.cos(angle)*pt[1]+math.sin(angle)*pt[2],-math.sin(angle)*pt[1]+math.cos(angle)*pt[2]})
end

function mat_sum(pt1,pt2)
    return {pt1[1]+pt2[1],pt1[2]+pt2[2]}
end

function check_ori(pt1,ref)
    if math.abs(ref[2]-pt1[2])>w/4 then
        return {0,0,90/180*math.pi}
    else
        return {0,0,180/180*math.pi}
    end
end

function cppon(posi,point,name)
    ori=check_ori(posi,point)
    size_bar={0.1,w,1}
    obj = simCreatePureShape(0,10110,size_bar,1,nil)
    --simSetObjectParent(obj,nil,-1)
    simSetObjectPosition(obj,-1,posi)
    simSetObjectOrientation(obj,-1,ori)
    simSetObjectName(obj,name)
    simSetObjectSpecialProperty(obj,sim_objectspecialproperty_detectable_all)
end

function cal_block_point(point,LSR,angle)
    if LSR==-1 then
        block_angle={angle+math.pi,angle+math.pi/2}
    elseif LSR==0 then
        block_angle={angle-math.pi/2,angle+math.pi/2}
    elseif LSR==1 then
        block_angle={angle+math.pi,angle-math.pi/2}
    end
    
    pt_b1 = mat_sum(point,rotate_point({0,w/2},block_angle[1]))
    table.insert(pt_b1,0.5)
    pt_b2 = mat_sum(point,rotate_point({0,w/2},block_angle[2]))
    table.insert(pt_b2,0.5)    

    return pt_b1,pt_b2
end


function make_block(point,LSR,dir,num)
    cur_dir=(dir+LSR)%4
    angle=(cur_dir+1)*math.pi/2
    point_next=mat_sum(point,rotate_point({0,w},angle))
    table.insert(point_next,0.5)    
    
    pt_b1,pt_b2=cal_block_point(point,LSR,angle)
    
    cppon(pt_b1,point,'Block_'..num..'_1')
    --simAddStatusbarMessage(pt_b1)
    --simAddStatusbarMessage(pt_b1)
    cppon(pt_b2,point,'Block_'..num..'_2')
    return point_next,cur_dir
end

function mapUpdate()
    point,dir=make_block(posi0,0,ini_dir,1)
    for i=2,#map do
        point,dir=make_block(point,map[i],dir,i)
    end
end

if (sim_call_type==sim_childscriptcall_initialization) then
    w=1.2
    --{-1:left 0:straight 1:right}
    map={0,0,1,-1,1,-1,0,1,1,0,-1,-1,0,1}
    start=simGetObjectAssociatedWithScript(sim_handle_self)
    posi0=simGetObjectPosition(start,-1)
    posi0[3]=0.5
    --{N:0 E:1 S:2 W:3}
    ini_dir=2
    simSetObjectOrientation(start,start,{0,0,math.pi/2*ini_dir})
    mapUpdate()
end


if (sim_call_type==sim_childscriptcall_actuation) then

    -- Put your main ACTUATION code here

    -- For example:
    --
    -- local position=simGetObjectPosition(handle,-1)
    -- position[1]=position[1]+0.001
    -- simSetObjectPosition(handle,-1,position)

end


if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here

end


if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here

end

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.IO;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Media3D;
using System.Collections;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Windows.Forms.DataVisualization.Charting;


namespace V_REP_CONVERTER
{
    public partial class Form1 : Form
    {
        int legs = 0;
        bool Enalbe_3d_button = false;
        List<double[]> points = new List<double[]>();
        string path = System.Windows.Forms.Application.StartupPath;
        
        Get_Point getP = new Get_Point();
        Cal_Method cal = new Cal_Method();

        public Form1()
        {
            Debug.WriteLine(path);
            InitializeComponent();
        }

        string path_modify(string path,int rid_Num)
        {
            string[] p_each = path.Split('\\');
            string modified_path = "";
            for (int i = 0; i < p_each.Length - rid_Num; i++)
            {
                modified_path += p_each[i]+"\\";
            }
            modified_path=modified_path.Remove(modified_path.Length-1);
            Debug.WriteLine(modified_path);
            return modified_path;
        }
        
        private void get_2d_data_click(object sender, EventArgs e)
        {
            points= new List<double[]>();
            OpenFileDialog theDialog = new OpenFileDialog();    //file upload window
            theDialog.Title = "Open Text File";
            theDialog.Filter = "TXT files|*.txt";               //file upload 창에서 text file만 보이도록

            theDialog.InitialDirectory = @path_modify(path, 2)+"\\data_files";
            
            if (theDialog.ShowDialog() == DialogResult.OK)
            {
                string[] lines = System.IO.File.ReadAllLines(theDialog.FileName);
                string text = System.IO.File.ReadAllText(theDialog.FileName);
                textBox1.Text = text;
     
                foreach (string line in lines)
                {
                    string[] s = line.Split(',');                                      
                    double[] temp = { Double.Parse(s[1]), Double.Parse(s[2]) };    // Parse: 문자형 -> 숫자형으로 형변환
                    points.Add(temp);
                }
                Enalbe_3d_button = true;
            }
        }
        private void leg_8_Click(object sender, EventArgs e)
        {
            if (Enalbe_3d_button == true)
            {
                To3D.Enabled = true;
            }
            legs = 8;
        }
        private void leg_12_Click(object sender, EventArgs e)
        {
            if (Enalbe_3d_button == true)
            {
                To3D.Enabled = true;
            }
            legs = 12;
        }

        /// <summary>
        /// 3D Data로 전환
        /// </summary>
        private void To_3D_click(object sender, EventArgs e)
        {
            if (legs == 8)
            {
                string[] Joint = { "Joint1_c_1_1", "Joint1_r_2_1", "Joint1_r_2_2", "Joint1_l_2_1", "Joint1_l_2_2", "Joint1_r_3_1", "Joint1_l_3_1", "Joint1_r_4_1", "Joint1_r_4_2", "Joint1_l_4_1", "Joint1_l_4_2", "Joint1_r_5_1", "Joint1_l_5_1", "Joint1_r_6_1", "Joint1_l_6_1", "Joint1_r_7_1", "Joint1_r_7_2", "Joint1_l_7_1", "Joint1_l_7_2", "Joint2_r_2_1", "Joint2_r_2_2", "Joint2_l_2_1", "Joint2_l_2_2", "Joint2_r_3_1", "Joint2_l_3_1", "Joint2_r_4_1", "Joint2_r_4_2", "Joint2_l_4_1", "Joint2_l_4_2", "Joint2_r_5_1", "Joint2_l_5_1", "Joint2_r_6_1", "Joint2_l_6_1", "Joint2_r_7_1", "Joint2_r_7_2", "Joint2_l_7_1", "Joint2_l_7_2", "Joint3_c_1_1", "Joint3_r_2_1", "Joint3_r_2_2", "Joint3_l_2_1", "Joint3_l_2_2", "Joint3_r_3_1", "Joint3_l_3_1", "Joint3_r_4_1", "Joint3_r_4_2", "Joint3_l_4_1", "Joint3_l_4_2", "Joint3_r_5_1", "Joint3_l_5_1", "Joint3_r_6_1", "Joint3_l_6_1", "Joint3_r_7_1", "Joint3_r_7_2", "Joint3_l_7_1", "Joint3_l_7_2", "Joint4_r_2_1", "Joint4_r_2_2", "Joint4_l_2_1", "Joint4_l_2_2", "Joint4_r_3_1", "Joint4_l_3_1", "Joint4_r_4_1", "Joint4_r_4_2", "Joint4_l_4_1", "Joint4_l_4_2", "Joint4_r_5_1", "Joint4_l_5_1", "Joint4_r_6_1", "Joint4_l_6_1", "Joint4_r_7_1", "Joint4_r_7_2", "Joint4_l_7_1", "Joint4_l_7_2" };
                string[] Jointorientation = { "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0" };
                string[] JointYvalue = new string[74];
                string[] Bar = { "bar1_c_1", "bar1_c_2", "bar1_r_3", "bar1_l_3", "bar1_r_4", "bar1_l_4", "bar1_r_5", "bar1_l_5", "bar1_r_6", "bar1_l_6", "bar1_r_7", "bar1_l_7", "bar1_r_8", "bar1_l_8", "bar1_r_9", "bar1_l_9", "bar1_r_10", "bar1_l_10", "bar2_c_1", "bar2_c_2", "bar2_r_3", "bar2_l_3", "bar2_r_4", "bar2_l_4", "bar2_r_5", "bar2_l_5", "bar2_r_6", "bar2_l_6", "bar2_r_7", "bar2_l_7", "bar2_r_8", "bar2_l_8", "bar2_r_9", "bar2_l_9", "bar2_r_10", "bar2_l_10", "bar3_c_1", "bar3_c_2", "bar3_r_3", "bar3_l_3", "bar3_r_4", "bar3_l_4", "bar3_r_5", "bar3_l_5", "bar3_r_6", "bar3_l_6", "bar3_r_7", "bar3_l_7", "bar3_r_8", "bar3_l_8", "bar3_r_9", "bar3_l_9", "bar3_r_10", "bar3_l_10", "bar4_c_1", "bar4_c_2", "bar4_r_3", "bar4_l_3", "bar4_r_4", "bar4_l_4", "bar4_r_5", "bar4_l_5", "bar4_r_6", "bar4_l_6", "bar4_r_7", "bar4_l_7", "bar4_r_8", "bar4_l_8", "bar4_r_9", "bar4_l_9", "bar4_r_10", "bar4_l_10" };
                string[] BarYvalue = new string[72];
                string[] Barorientation = { };
                string[] Dummy = { "Dummy1A", "Dummy1A0", "Dummy1A2", "Dummy1A3", "Dummy1B", "Dummy1B0", "Dummy1B2", "Dummy1B3", "Dummy1A1", "Dummy1B1", "Dummy1A4", "Dummy1B4", "Dummy2A", "Dummy2A0", "Dummy2A2", "Dummy2A3", "Dummy2B", "Dummy2B0", "Dummy2B2", "Dummy2B3", "Dummy2A1", "Dummy2B1", "Dummy2A4", "Dummy2B4", "Dummy3A", "Dummy3A0", "Dummy3A2", "Dummy3A3", "Dummy3B", "Dummy3B0", "Dummy3B2", "Dummy3B3", "Dummy3A1", "Dummy3B1", "Dummy3A4", "Dummy3B4", "Dummy4A", "Dummy4A0", "Dummy4A2", "Dummy4A3", "Dummy4B", "Dummy4B0", "Dummy4B2", "Dummy4B3", "Dummy4A1", "Dummy4B1", "Dummy4B4", "Dummy4B4" };
                string[] Sphere = { "Sphere1_1", "Sphere1_2", "Sphere2_1", "Sphere2_2", "Sphere3_1", "Sphere3_2", "Sphere4_1", "Sphere4_2" };
                string[] Force_sensor = { "Force_sensor1_1", "Force_sensor1_2", "Force_sensor2_1", "Force_sensor2_2", "Force_sensor3_1", "Force_sensor3_2", "Force_sensor4_1", "Force_sensor4_2" };
                
                double[][] point_r = new double[8][];
                double[][] point_l = new double[8][];

                double[][] p0 = getP.GetP(points.ToArray(), 90);
                double[][] p180 = getP.GetP(points.ToArray(), 270);

                System.IO.StreamWriter writer;
                writer = File.CreateText(@"C:\\Program Files\V-REP3\data_files\3D_8legs.txt");


                #region Joints
                double[] Joint_Y_default = { 0.0025, 0.015, 0.015, 0.015, 0.015, 0.02, 0.0175, 0.01, 0.01, 0.01, 0.01, 0.015, 0.015, 0.0125, 0.0125, 0.0175, 0.0175, 0.0175, 0.0175 }; // 19개
                double[] Joint_Y_modifier = { 0, -0.1, -0.3, -0.4};

                int Joint_Y_index = 0;
                int current_Joint = 0;

                int point_Number = 0; // initialize  k = point number

                for (int i = 0; i < Joint.Length; i++)
                {
                    if (current_Joint != (int)Char.GetNumericValue(Joint[i][5]))
                    {
                        current_Joint++;
                        {
                            switch (current_Joint)
                            {
                                case 1:
                                case 3:
                                    point_r = p0;
                                    point_l = p0;
                                    Joint_Y_index = 0;
                                    break;
                                default:
                                    point_r = p180;
                                    point_l = p180;
                                    Joint_Y_index = 1;
                                    break;
                            }
                        }
                    }

                    JointYvalue[i] = (Joint_Y_default[Joint_Y_index] + Joint_Y_modifier[current_Joint - 1]).ToString();
                    point_Number = (int)Char.GetNumericValue(Joint[i][9]) - 1;

                    if (Joint[i][7] == Convert.ToChar("r"))
                    {   // X   Y  Z  & Orientation
                        writer.WriteLine(Joint[i] + "," + (-point_r[point_Number][0]).ToString() + "," + JointYvalue[i] + "," + point_r[point_Number][1].ToString() + "," + Jointorientation[i]);
                    }
                    else
                    {
                        writer.WriteLine(Joint[i] + "," + point_l[point_Number][0].ToString() + "," + JointYvalue[i] + "," + point_l[point_Number][1].ToString() + "," + Jointorientation[i]);
                    }
                    Joint_Y_index++;
                }
                #endregion

                #region Bar
                double[] Bar_Y_default = { 0, 0.005, 0.02, 0.015, 0.015, 0.02, 0.015, 0.02, 0.02, 0.015, 0.01, 0.025, 0.025, 0.01, 0.015, 0.02, 0.01, 0.025 };
                double[] Bar_Y_modifier = { 0, -0.1, -0.3, -0.4}; //Bar_Y_modifier[current_Bar-1]

                int[,] Bar_Joint_combination = new int[10, 2] { { 0, 0 }, { 1, 0 }, { 1, 2 }, { 3, 2 }, { 4, 3 }, { 6, 3 }, { 5, 4 }, { 6, 1 }, { 6, 5 }, { 7, 6 } };
                int Bar_Y_index = 0;
                int current_Bar = 0;
                int Bar_Number = 0;
                int Bar_point1 = 0;
                int Bar_point2 = 0;

                for (int i = 0; i < Bar.Length; i++)
                {
                    if (current_Bar != (int)Char.GetNumericValue(Bar[i][3]))
                    {
                        current_Bar++;
                        Bar_Y_index = 0;
                        switch (current_Bar)
                        {
                            case 1:
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            default:
                                point_r = p180;
                                point_l = p180;
                                break;
                        }
                    }

                    //position
                    Bar_Number = (int)Char.GetNumericValue(Bar[i][7]);

                    if (Bar[i].Length == 9)
                    {   //10일 경우 bar #1로 알아먹는거 방지
                        Bar_Number = 10;
                    }
                    Bar_point1 = Bar_Joint_combination[Bar_Number - 1, 0];
                    Bar_point2 = Bar_Joint_combination[Bar_Number - 1, 1];
                    //Yvalue
                    BarYvalue[i] = (Bar_Y_default[Bar_Y_index] + Bar_Y_modifier[current_Bar - 1]).ToString();

                    if (Bar[i][5] == 'l')
                    {
                        writer.WriteLine(Bar[i] + "," + ((point_l[Bar_point1][0] + point_l[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_l[Bar_point1][1] + point_l[Bar_point2][1]) / 2).ToString() + ",1.570796327,0," + cal.slope_cal(point_l[Bar_point1], point_l[Bar_point2]) + "," + (cal.distance_cal(point_l[Bar_point1], point_l[Bar_point2]) + 0.0125));
                    }
                    else if (Bar[i][5] == 'r' || Bar[i][7] == '2')
                    {
                        writer.WriteLine(Bar[i] + "," + (-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2).ToString() + ",-1.570796327,0," + cal.slope_cal(point_r[Bar_point1], point_r[Bar_point2]) + "," + (cal.distance_cal(point_r[Bar_point1], point_r[Bar_point2]) + 0.0125));
                    }
                    else if (Bar[i][7] == '1')//Bar#_c_1
                    {
                        writer.WriteLine(Bar[i] + "," + (-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2).ToString() + ",-1.570796327,0,0," + ((point_l[3][0] * 2) + 0.0125));
                    }
                    Bar_Y_index++;
                }

                //bar_u
                for (int i = 0; i < 1; i++)
                {
                    if (i == 0)
                    {
                        writer.WriteLine("bar_u,0,-0.2," + (p0[i + 1][1] + 0.04) + ",0,0,1.570796327,0.605");
                        writer.WriteLine("bar1_u,0,0," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar2_u,0,-0.1," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar3_u,0,-0.3," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar4_u,0,-0.4," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                    }
                }
                #endregion

                #region dummy
                int current_Dummy = 0;

                for (int i = 0; i < Dummy.Length; i++)
                {
                    if (current_Dummy != (int)Char.GetNumericValue(Dummy[i][5]))
                    {
                        current_Dummy++;
                        switch (current_Dummy)
                        {
                            case 1:
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            default:
                                point_r = p180;
                                point_l = p180;
                                break;
                        }
                    }
                    if (Dummy[i].Length == 7)
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[1][0] + "," + point_l[1][1]);
                    }
                    else if (Dummy[i][7] == '1')
                    {
                        writer.WriteLine(Dummy[i] + "," + -point_r[4][0] + "," + point_r[4][1]);
                    }
                    else if (Dummy[i][7] == '4')
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[4][0] + "," + point_l[4][1]);
                    }
                    else
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[1][0] + "," + point_l[1][1]);
                    }
                }
                #endregion

                #region Sphere & forcesensor
                //Sphere+forcesensor
                int current_Sphere = 0;
                for (int i = 0; i < Sphere.Length; i++)
                {
                    if (current_Sphere != (int)Char.GetNumericValue(Sphere[i][6]))
                    {
                        current_Sphere++;
                        switch (current_Sphere)
                        {
                            case 1:
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            default:
                                point_r = p180;
                                point_l = p180;
                                break;
                        }
                    }
                    if (Sphere[i][8] == '1')
                    {
                        writer.WriteLine(Sphere[i] + "," + point_l[7][0] + "," + point_l[7][1]);
                    }
                    else
                    {
                        writer.WriteLine(Sphere[i] + "," + -point_r[7][0] + "," + point_r[7][1]);
                    }
                }

                int current_Sensor = 0;
                for (int i = 0; i < Force_sensor.Length; i++)
                {
                    if (current_Sensor != (int)Char.GetNumericValue(Force_sensor[i][12]))
                    {
                        current_Sensor++;
                        switch (current_Sensor)
                        {
                            case 1:
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            default:
                                point_r = p180;
                                point_l = p180;
                                break;
                        }
                    }
                    if (Force_sensor[i][14] == '1')
                    {
                        writer.WriteLine(Force_sensor[i] + "," + point_l[7][0] + "," + point_l[7][1]);
                    }
                    else
                    {
                        writer.WriteLine(Force_sensor[i] + "," + -point_r[7][0] + "," + point_r[7][1]);
                    }
                }

                #endregion

                writer.Close();
            }
            if (legs == 12)
            {
                //Joint 110개, 각 18개 + J1,J3 한개씩
                string[] Joint = { "Joint1_c_1_1", "Joint1_r_2_1", "Joint1_r_2_2", "Joint1_l_2_1", "Joint1_l_2_2", "Joint1_r_3_1", "Joint1_l_3_1", "Joint1_r_4_1", "Joint1_r_4_2", "Joint1_l_4_1", "Joint1_l_4_2", "Joint1_r_5_1", "Joint1_l_5_1", "Joint1_r_6_1", "Joint1_l_6_1", "Joint1_r_7_1", "Joint1_r_7_2", "Joint1_l_7_1", "Joint1_l_7_2", "Joint2_r_2_1", "Joint2_r_2_2", "Joint2_l_2_1", "Joint2_l_2_2", "Joint2_r_3_1", "Joint2_l_3_1", "Joint2_r_4_1", "Joint2_r_4_2", "Joint2_l_4_1", "Joint2_l_4_2", "Joint2_r_5_1", "Joint2_l_5_1", "Joint2_r_6_1", "Joint2_l_6_1", "Joint2_r_7_1", "Joint2_r_7_2", "Joint2_l_7_1", "Joint2_l_7_2", "Joint3_c_1_1", "Joint3_r_2_1", "Joint3_r_2_2", "Joint3_l_2_1", "Joint3_l_2_2", "Joint3_r_3_1", "Joint3_l_3_1", "Joint3_r_4_1", "Joint3_r_4_2", "Joint3_l_4_1", "Joint3_l_4_2", "Joint3_r_5_1", "Joint3_l_5_1", "Joint3_r_6_1", "Joint3_l_6_1", "Joint3_r_7_1", "Joint3_r_7_2", "Joint3_l_7_1", "Joint3_l_7_2", "Joint4_r_2_1", "Joint4_r_2_2", "Joint4_l_2_1", "Joint4_l_2_2", "Joint4_r_3_1", "Joint4_l_3_1", "Joint4_r_4_1", "Joint4_r_4_2", "Joint4_l_4_1", "Joint4_l_4_2", "Joint4_r_5_1", "Joint4_l_5_1", "Joint4_r_6_1", "Joint4_l_6_1", "Joint4_r_7_1", "Joint4_r_7_2", "Joint4_l_7_1", "Joint4_l_7_2", "Joint5_r_2_1", "Joint5_r_2_2", "Joint5_l_2_1", "Joint5_l_2_2", "Joint5_r_3_1", "Joint5_l_3_1", "Joint5_r_4_1", "Joint5_r_4_2", "Joint5_l_4_1", "Joint5_l_4_2", "Joint5_r_5_1", "Joint5_l_5_1", "Joint5_r_6_1", "Joint5_l_6_1", "Joint5_r_7_1", "Joint5_r_7_2", "Joint5_l_7_1", "Joint5_l_7_2", "Joint6_r_2_1", "Joint6_r_2_2", "Joint6_l_2_1", "Joint6_l_2_2", "Joint6_r_3_1", "Joint6_l_3_1", "Joint6_r_4_1", "Joint6_r_4_2", "Joint6_l_4_1", "Joint6_l_4_2", "Joint6_r_5_1", "Joint6_l_5_1", "Joint6_r_6_1", "Joint6_l_6_1", "Joint6_r_7_1", "Joint6_r_7_2", "Joint6_l_7_1", "Joint6_l_7_2" };
                string[] Jointorientation = { "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,-3.141592741", "1.570796327,0,-3.141592741", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "-1.570796327,0,0", "-1.570796327,0,0", "1.570796327,0,0", "1.570796327,0,0" };
                string[] JointYvalue = new string[110];
                //bar 108개, 각 18개
                string[] Bar = { "bar1_c_1", "bar1_c_2", "bar1_r_3", "bar1_l_3", "bar1_r_4", "bar1_l_4", "bar1_r_5", "bar1_l_5", "bar1_r_6", "bar1_l_6", "bar1_r_7", "bar1_l_7", "bar1_r_8", "bar1_l_8", "bar1_r_9", "bar1_l_9", "bar1_r_10", "bar1_l_10", "bar2_c_1", "bar2_c_2", "bar2_r_3", "bar2_l_3", "bar2_r_4", "bar2_l_4", "bar2_r_5", "bar2_l_5", "bar2_r_6", "bar2_l_6", "bar2_r_7", "bar2_l_7", "bar2_r_8", "bar2_l_8", "bar2_r_9", "bar2_l_9", "bar2_r_10", "bar2_l_10", "bar3_c_1", "bar3_c_2", "bar3_r_3", "bar3_l_3", "bar3_r_4", "bar3_l_4", "bar3_r_5", "bar3_l_5", "bar3_r_6", "bar3_l_6", "bar3_r_7", "bar3_l_7", "bar3_r_8", "bar3_l_8", "bar3_r_9", "bar3_l_9", "bar3_r_10", "bar3_l_10", "bar4_c_1", "bar4_c_2", "bar4_r_3", "bar4_l_3", "bar4_r_4", "bar4_l_4", "bar4_r_5", "bar4_l_5", "bar4_r_6", "bar4_l_6", "bar4_r_7", "bar4_l_7", "bar4_r_8", "bar4_l_8", "bar4_r_9", "bar4_l_9", "bar4_r_10", "bar4_l_10", "bar5_c_1", "bar5_c_2", "bar5_r_3", "bar5_l_3", "bar5_r_4", "bar5_l_4", "bar5_r_5", "bar5_l_5", "bar5_r_6", "bar5_l_6", "bar5_r_7", "bar5_l_7", "bar5_r_8", "bar5_l_8", "bar5_r_9", "bar5_l_9", "bar5_r_10", "bar5_l_10", "bar6_c_1", "bar6_c_2", "bar6_r_3", "bar6_l_3", "bar6_r_4", "bar6_l_4", "bar6_r_5", "bar6_l_5", "bar6_r_6", "bar6_l_6", "bar6_r_7", "bar6_l_7", "bar6_r_8", "bar6_l_8", "bar6_r_9", "bar6_l_9", "bar6_r_10", "bar6_l_10" };
                string[] BarYvalue = new string[108];
                string[] Barorientation = { };
                //Dummy 72개, 각 12개
                string[] Dummy = { "Dummy1A", "Dummy1A0", "Dummy1A2", "Dummy1A3", "Dummy1B", "Dummy1B0", "Dummy1B2", "Dummy1B3", "Dummy1A1", "Dummy1B1", "Dummy1A4", "Dummy1B4", "Dummy2A", "Dummy2A0", "Dummy2A2", "Dummy2A3", "Dummy2B", "Dummy2B0", "Dummy2B2", "Dummy2B3", "Dummy2A1", "Dummy2B1", "Dummy2A4", "Dummy2B4", "Dummy3A", "Dummy3A0", "Dummy3A2", "Dummy3A3", "Dummy3B", "Dummy3B0", "Dummy3B2", "Dummy3B3", "Dummy3A1", "Dummy3B1", "Dummy3A4", "Dummy3B4", "Dummy4A", "Dummy4A0", "Dummy4A2", "Dummy4A3", "Dummy4B", "Dummy4B0", "Dummy4B2", "Dummy4B3", "Dummy4A1", "Dummy4B1", "Dummy4B4", "Dummy4B4", "Dummy5A", "Dummy5A0", "Dummy5A2", "Dummy5A3", "Dummy5B", "Dummy5B0", "Dummy5B2", "Dummy5B3", "Dummy5A1", "Dummy5B1", "Dummy5A4", "Dummy5B4", "Dummy6A", "Dummy6A0", "Dummy6A2", "Dummy6A3", "Dummy6B", "Dummy6B0", "Dummy6B2", "Dummy6B3", "Dummy6A1", "Dummy6B1", "Dummy6A4", "Dummy6B4" };
                string[] Sphere = { "Sphere1_1", "Sphere1_2", "Sphere2_1", "Sphere2_2", "Sphere3_1", "Sphere3_2", "Sphere4_1", "Sphere4_2", "Sphere5_1", "Sphere5_2", "Sphere6_1", "Sphere6_2" };
                string[] Force_sensor = { "Force_sensor1_1", "Force_sensor1_2", "Force_sensor2_1", "Force_sensor2_2", "Force_sensor3_1", "Force_sensor3_2", "Force_sensor4_1", "Force_sensor4_2", "Force_sensor5_1", "Force_sensor5_2", "Force_sensor6_1", "Force_sensor6_2" };
                
                double[][] point_r = new double[8][];
                double[][] point_l = new double[8][];
                double[][] p0 = getP.GetP(points.ToArray(), 90);
                double[][] p120 = getP.GetP(points.ToArray(), 210);
                double[][] p240 = getP.GetP(points.ToArray(), 330);
                

                System.IO.StreamWriter writer;
                writer = File.CreateText(@"C:\\Program Files\V-REP3\data_files\3D_12leg.txt");
                

                #region Joints
                double[] Joint_Y_default = { 0.0025, 0.015, 0.015, 0.015, 0.015, 0.02, 0.0175, 0.01, 0.01, 0.01, 0.01, 0.015, 0.015, 0.0125, 0.0125, 0.0175, 0.0175, 0.0175, 0.0175 }; // 19개
                double[] Joint_Y_modifier = { 0, -0.1, -0.3, -0.4, 0.1, -0.5 };

                int Joint_Y_index = 0;
                int current_Joint = 0;

                int point_Number = 0; // initialize  k = point number

                for (int i = 0; i < Joint.Length; i++)
                {
                    if (current_Joint != (int)Char.GetNumericValue(Joint[i][5]))
                    {
                        current_Joint++;
                        switch (current_Joint)
                        {
                            case 1:
                                point_r = p120;
                                point_l = p240;
                                Joint_Y_index = 0;
                                break;
                            case 2:
                                point_r = p0;
                                point_l = p0;
                                Joint_Y_index = 1;
                                break;
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                Joint_Y_index = 0;
                                break;
                            case 4:
                                point_r = p120;
                                point_l = p240;
                                Joint_Y_index = 1;
                                break;
                            case 5:
                                point_r = p240;
                                point_l = p120;
                                Joint_Y_index = 1;
                                break;
                            case 6:
                                point_r = p240;
                                point_l = p120;
                                Joint_Y_index = 1;
                                break;
                            default:
                                Joint_Y_index = 1;
                                break;
                        }
                    }

                    JointYvalue[i] = (Joint_Y_default[Joint_Y_index] + Joint_Y_modifier[current_Joint - 1]).ToString();
                    point_Number = (int)Char.GetNumericValue(Joint[i][9]) - 1;

                    if (Joint[i][7] == Convert.ToChar("r"))
                    {   // X   Y  Z  & Orientation
                        writer.WriteLine(Joint[i] + "," + (-point_r[point_Number][0]).ToString() + "," + JointYvalue[i] + "," + point_r[point_Number][1].ToString() + "," + Jointorientation[i]);
                        //    Debug.WriteLine(Joint[i] + "//" + (100 * Math.Round((-point_r[point_Number][0]), 3)).ToString() + "," + (100 * Math.Round(point_r[point_Number][1], 3)).ToString() +"  //  "+JointYvalue[i].ToString());

                    }
                    else
                    {
                        writer.WriteLine(Joint[i] + "," + point_l[point_Number][0].ToString() + "," + JointYvalue[i] + "," + point_l[point_Number][1].ToString() + "," + Jointorientation[i]);
                        //    Debug.WriteLine(Joint[i] + "//" + (100 * Math.Round((point_l[point_Number][0]), 3)).ToString() + "," + (100 * Math.Round(point_l[point_Number][1], 3)).ToString() + "  //  "+ JointYvalue[i].ToString());
                    }
                    Joint_Y_index++;
                }
                #endregion

                #region Bar
                double[] Bar_Y_default = { 0, 0.005, 0.02, 0.015, 0.015, 0.02, 0.015, 0.02, 0.02, 0.015, 0.01, 0.025, 0.025, 0.01, 0.015, 0.02, 0.01, 0.025 };
                double[] Bar_Y_modifier = { 0, -0.1, -0.3, -0.4, 0.1, -0.5 }; //Bar_Y_modifier[current_Bar-1]

                int[,] Bar_Joint_combination = new int[10, 2] { { 0, 0 }, { 1, 0 }, { 1, 2 }, { 3, 2 }, { 4, 3 }, { 6, 3 }, { 5, 4 }, { 6, 1 }, { 6, 5 }, { 7, 6 } };
                int Bar_Y_index = 0;
                int current_Bar = 0;
                int Bar_Number = 0;
                int Bar_point1 = 0;
                int Bar_point2 = 0;

                for (int i = 0; i < 108; i++)
                {
                    if (current_Bar != (int)Char.GetNumericValue(Bar[i][3]))
                    {
                        current_Bar++;
                        Bar_Y_index = 0;
                        switch (current_Bar)
                        {
                            case 1:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 2:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 4:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 5:
                                point_r = p240;
                                point_l = p120;
                                break;
                            case 6:
                                point_r = p240;
                                point_l = p120;
                                break;
                        }
                    }
                        
                    //position
                    Bar_Number = (int)Char.GetNumericValue(Bar[i][7]);

                    if (Bar[i].Length == 9)
                    {   //10일 경우 bar #1로 알아먹는거 방지
                        Bar_Number = 10;
                    }
                    Bar_point1 = Bar_Joint_combination[Bar_Number - 1, 0];
                    Bar_point2 = Bar_Joint_combination[Bar_Number - 1, 1];
                    //Yvalue
                    BarYvalue[i] = (Bar_Y_default[Bar_Y_index] + Bar_Y_modifier[current_Bar - 1]).ToString();

                    if (Bar[i][5] == 'l')
                    {
                        writer.WriteLine(Bar[i] + "," + ((point_l[Bar_point1][0] + point_l[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_l[Bar_point1][1] + point_l[Bar_point2][1]) / 2).ToString() + ",1.570796327,0," + cal.slope_cal(point_l[Bar_point1], point_l[Bar_point2]) + "," + (cal.distance_cal(point_l[Bar_point1], point_l[Bar_point2]) + 0.0125));
                        //Debug.WriteLine(Bar[i]+"//"+(100 * Math.Round(((point_l[Bar_point1][0] + point_l[Bar_point2][0]) / 2), 3)).ToString() + "," + (100 * Math.Round(((point_l[Bar_point1][1] + point_l[Bar_point2][1]) / 2), 3)).ToString() + "           ////" + Math.Round(-cal.slope_cal(point_l[Bar_point1], point_l[Bar_point2]) * 180 / Math.PI,3));
                    }
                    else if (Bar[i][5] == 'r' || Bar[i][7] == '2')
                    {
                        writer.WriteLine(Bar[i] + "," + (-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2).ToString() + ",-1.570796327,0," + cal.slope_cal(point_r[Bar_point1], point_r[Bar_point2]) + "," + (cal.distance_cal(point_r[Bar_point1], point_r[Bar_point2]) + 0.0125));
                        //if (Bar[i][7] == '2')
                        //{
                        //    Debug.WriteLine(Bar[i] + "//" + (100 * Math.Round((-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2), 3)).ToString() + "," + (100 * Math.Round(((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2), 3)).ToString() + "           ////" + Math.Round(-cal.slope_cal(point_r[Bar_point1], point_r[Bar_point2])) * 180 / Math.PI);
                        //}
                    }
                    else if (Bar[i][7] == '1')//Bar#_c_1
                    {
                        writer.WriteLine(Bar[i] + "," + (-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2).ToString() + "," + BarYvalue[i] + "," + ((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2).ToString() + ",-1.570796327,0,0," + ((point_l[3][0] * 2) + 0.0125));
                        //Debug.WriteLine(Bar[i] + "//" + (100 * Math.Round((-(point_r[Bar_point1][0] + point_r[Bar_point2][0]) / 2), 3)).ToString() + "," + (100 * Math.Round(((point_r[Bar_point1][1] + point_r[Bar_point2][1]) / 2), 3)).ToString());
                    }
                    Bar_Y_index++;
                }

                //bar_u 수정 필요 bar_u
                for (int i = 0; i < 1; i++)
                {
                    if (i == 0)
                    {
                        writer.WriteLine("bar_u,0,-0.2," + (p0[i + 1][1] + 0.04) + ",0,0,1.570796327,0.605");
                        writer.WriteLine("bar1_u,0,0," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar2_u,0,-0.1," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar3_u,0,-0.3," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar4_u,0,-0.4," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar5_u,0,0.1," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));
                        writer.WriteLine("bar6_u,0,-0.5," + (p0[i + 1][1]) + ",1.570796327,0,1.570796327," + (p0[i + 1][1] + 0.03625 - p0[i][1]));

                    }
                }
                #endregion

                #region dummy
                int current_Dummy = 0;

                for (int i = 0; i < 72; i++)
                {
                    if (current_Dummy != (int)Char.GetNumericValue(Dummy[i][5]))
                    {
                        current_Dummy++;
                        switch (current_Dummy)
                        {
                            case 1:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 2:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 4:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 5:
                                point_r = p240;
                                point_l = p120;
                                break;
                            case 6:
                                point_r = p240;
                                point_l = p120;
                                break;
                        }
                    }
                    if (Dummy[i].Length == 7)
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[1][0] + "," + point_l[1][1]);
                    }
                    else if (Dummy[i][7] == '1')
                    {
                        writer.WriteLine(Dummy[i] + "," + -point_r[4][0] + "," + point_r[4][1]);
                    }
                    else if (Dummy[i][7] == '4')
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[4][0] + "," + point_l[4][1]);
                    }
                    else
                    {
                        writer.WriteLine(Dummy[i] + "," + point_l[1][0] + "," + point_l[1][1]);
                    }
                }
                #endregion

                #region Sphere & forcesensor
                //Sphere+forcesensor
                int current_Sphere = 0;
                for (int i = 0; i < 12; i++)
                {
                    if (current_Sphere != (int)Char.GetNumericValue(Sphere[i][6]))
                    {
                        current_Sphere++;
                        switch (current_Sphere)
                        {
                            case 1:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 2:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 4:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 5:
                                point_r = p240;
                                point_l = p120;
                                break;
                            case 6:
                                point_r = p240;
                                point_l = p120;
                                break;
                        }
                    }
                    if (Sphere[i][8] == '1')
                    {
                        writer.WriteLine(Sphere[i] + "," + point_l[7][0] + "," + point_l[7][1]);
                    }
                    else
                    {
                        writer.WriteLine(Sphere[i] + "," + -point_r[7][0] + "," + point_r[7][1]);
                    }
                }

                int current_Sensor = 0;
                for (int i = 0; i < 12; i++)
                {
                    if (current_Sensor != (int)Char.GetNumericValue(Force_sensor[i][12]))
                    {
                        current_Sensor++;
                        switch (current_Sensor)
                        {
                            case 1:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 2:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 3:
                                point_r = p0;
                                point_l = p0;
                                break;
                            case 4:
                                point_r = p120;
                                point_l = p240;
                                break;
                            case 5:
                                point_r = p240;
                                point_l = p120;
                                break;
                            case 6:
                                point_r = p240;
                                point_l = p120;
                                break;
                        }
                    }
                    if (Force_sensor[i][14] == '1')
                    {
                        writer.WriteLine(Force_sensor[i] + "," + point_l[7][0] + "," + point_l[7][1]);
                    }
                    else
                    {
                        writer.WriteLine(Force_sensor[i] + "," + -point_r[7][0] + "," + point_r[7][1]);
                    }
                }

                #endregion

                writer.Close();
            }
            is3D.Visible = true;
        }

        private void button2_Click(object sender, EventArgs e)
        {
            string path_vrep = "C:\\Program Files\\V-REP3\\V-REP_PRO_EDU\\vrep.exe";
            string model_file = "../models/";
            if (legs == 8)
            {
                model_file += "8leg.ttt";
            }
            else
            {
                model_file += "12leg.ttt";
            }

            Process vrep = new Process()
            {
                //StartInfo = new ProcessStartInfo(path_vrep, "-s10000 -q " +model_file)
                StartInfo = new ProcessStartInfo(path_vrep, "" + model_file)
                {
                    WindowStyle = ProcessWindowStyle.Normal,
                    WorkingDirectory = Path.GetDirectoryName(path_vrep),
                    // The following commands are needed to redirect the standard output.
                    // This means that it will be redirected to the Process.StandardOutput StreamReader.
                    RedirectStandardOutput = true,
                    UseShellExecute = false,
                    CreateNoWindow = true
                }
            };
            
            try
            {
                vrep.Start();
                // Get the output into a string
                string result = vrep.StandardOutput.ReadToEnd();
                // Display the command output.
            }
            catch (Exception ex)
            {
                Debug.WriteLine("Error: " + ex.Message);
            }
        }



        private void button3_Click(object sender, EventArgs e)
        {           
            string[] lines = System.IO.File.ReadAllLines(@"C:\\Program Files\\V-REP3\\result\\result.txt");
            string text = System.IO.File.ReadAllText(@"C:\\Program Files\\V-REP3\\result\\result.txt");

            // textBox1.Text = text;
            //EX) points[0][0][0] = path x,y와 속도 값 , force값
            List<double[]> points = new List<double[]>();

            foreach (string line in lines)
            {
                string[] s = line.Split(',');
                double[] temp = { Double.Parse(s[0]), Double.Parse(s[1]), Double.Parse(s[2]), Double.Parse(s[3]), Double.Parse(s[4]), Double.Parse(s[5]) };
                points.Add(temp);
            }

            //평균속도 계산
            double sum = 0;
            for (int i = 0; i < 203; i++)
            {
                double s;
                s = points[i][2];
                sum = sum + s;
            }
            double velo = sum / 203;
            textBox4.Text = string.Format("{0:f3}", velo);

            //distance 설정
            textBox6.Text = string.Format("{0:f3}", points[202][0]);

            //Path그라프 설정
            chart1.Series.Clear();
            Series Path = chart1.Series.Add("Path");
            Path.ChartType = SeriesChartType.Line;
            for (int i = 0; i < 203; i++)
            {
                Path.Points.AddXY(string.Format("{0:f3}", points[i][0]), points[i][1]);
            }


            //force그라프 설정
            chart2.Series.Clear();
            Series Force = chart2.Series.Add("Force");
            Force.ChartType = SeriesChartType.Line;
            for (int i = 0; i < 203; i++)
            {
                Force.Points.AddXY(string.Format("{0:f3}", points[i][0]), points[i][3]);
            }


            //trajectory 그라프 설정
            chart3.Series.Clear();
            Series Trajectory = chart3.Series.Add("Trajectory");
            Trajectory.ChartType = SeriesChartType.Line;
            for (int i = 0; i < 203; i++)
            {
                Trajectory.Points.AddXY(points[i][4], points[i][5]);
            }

            //O,X 가능성
            if (points[202][0] < 0.3)
            {
                textBox5.Text = "X";
            }
            else
            {
                textBox5.Text = "O";
            }

        }
    }

}