/* Error tolerance for cutouts */
T = 0.2;

/* Arc smoothness */
R = 100;

/* PCB dimensions */
PCB_WIDTH = 66.5;
PCB_DEPTH = 66.5;
PCB_THICKNESS = 1.6;
PCB_OFFSET = 6;

/* Robot dimensions */
THICKNESS = 3;
WIDTH = 100;
HEIGHT = 90;
DEPTH = 100;
OFFSET = 34;
TILT = -8;

/* Ultrasonic sensor parameters */
ULTRASONIC_SENSOR_PLACEMENT = 47;
ULTRASONIC_SENSOR_SPAN = 26.5;
ULTRASONIC_SENSOR_HOLE = 16;

/* Line sensor parameters */
LINE_SENSOR_PLACEMENT = 13.5;
LINE_SENSOR_HOLES = 3;
LINE_SENSOR_SPAN = 38;

/* How far from eachother servo mount surfaces are placed */
SERVO_AXLE_LENGTH = 58;

COVER_WIDTH = 70;
COVER_HEIGHT = 76;

/* Servo parameters */
SERVO_MOUNT_PLACEMENT = 65;
SERVO_MOUNT_OFFSET = 33.5;

/* Magnet parameters */
MAGNET_HEIGHT = 0.8;
MAGNET_DIAMETER = 5;

/* Show placeholders for battery, wheels, PCB, etc */
DEBUG = false;

/* Helper function for mirror-copying object */
module mirror_copy() {
    children();
    mirror() {
        children();
    }
}

/* Mount holes for servos */
module mount_hole() {
    cylinder(h = 100, d = LINE_SENSOR_HOLES + T, $fn = R);
}

/* Standard servo cutout helper */
module servo_cutout() {
    translate([-34.5, 5, 0]) mount_hole();
    translate([-34.5, -5, 0]) mount_hole();
    translate([15, 5, 0]) mount_hole();
    translate([15, -5, 0]) mount_hole();
    translate([-31, -10, 0]) cube([40 + T * 2 + 2, 20 + T * 2, 100]);
}

module face() {
    difference() {
        union() {
            /* Front face */
            cube([WIDTH / 2, HEIGHT * 2, THICKNESS]);

            /* Ultrasonic sensor mount */
            rotate([TILT, 0, 0]) {
                translate([ULTRASONIC_SENSOR_SPAN / 2, ULTRASONIC_SENSOR_PLACEMENT, 0]) {
                    cylinder(h = 20, d = ULTRASONIC_SENSOR_HOLE + THICKNESS, $fn = R);
                }
            }
        }

        /* Line following sensor mounting holes */
        translate([0, LINE_SENSOR_PLACEMENT, -T]) {
            translate([LINE_SENSOR_SPAN, 0, 0]) {
                cylinder(h = THICKNESS + T * 2, d2 = LINE_SENSOR_HOLES + T, d1 = LINE_SENSOR_HOLES * 2 + T, $fn = R);
            }
            cylinder(h = THICKNESS + T * 2, d2 = LINE_SENSOR_HOLES + T, d1 = LINE_SENSOR_HOLES * 2 + T, $fn = R);
        }


        /* Ultrasonic sensor cutout */
        rotate([TILT, 0, 0]) {
            translate([0, ULTRASONIC_SENSOR_PLACEMENT, 0]) {
                translate([ULTRASONIC_SENSOR_SPAN / 2, 0, -1]) {
                    cylinder(h = 100, d = ULTRASONIC_SENSOR_HOLE + T, $fn = R);
                }

                translate([0, 0, 15]) {
                    cube([ULTRASONIC_SENSOR_SPAN / 2, ULTRASONIC_SENSOR_SPAN / 2, 30]);
                }
            }
        }
    }
}

module frame() {
    /* Front face to top face support arcs */
    rotate([0, 90, 0]) {
        translate([-90, 0, -WIDTH / 2]) {
            difference() {
                cylinder(d = 300, h = T + WIDTH/2 - COVER_WIDTH/2, $fn = R);
                translate([0, 0, -THICKNESS])
                cylinder(d = 176, h = WIDTH-COVER_WIDTH, $fn = R);

                /* Clear out line sensor holes */
                cube([200, 30, 20]);
                translate([-200, -200, -1]) {
                    cube([200, 400, 20]);
                }
            }
        }
    }

    difference() {
        translate([-COVER_WIDTH / 2, HEIGHT - OFFSET, 0]) {
            /* PCB compartment walls */
            cube([THICKNESS, OFFSET, DEPTH]);

            /* PCB rails */
            translate([0, OFFSET - PCB_OFFSET - THICKNESS * 3, 0]) {
                cube([6, PCB_OFFSET + THICKNESS * 3, DEPTH]);

            }
        }

        /* PCB cutout */
        translate([-PCB_WIDTH / 2, HEIGHT - 9, DEPTH - PCB_DEPTH - THICKNESS + T]) {
            cube([PCB_WIDTH + T, PCB_THICKNESS + T, PCB_DEPTH]);
            translate([0, -PCB_THICKNESS, -2]) {
                cube([PCB_WIDTH + T, PCB_THICKNESS + T, PCB_DEPTH]);
            }
        }

    }

    /* Servo mounting walls */
    translate([-SERVO_AXLE_LENGTH / 2, 0, 0]) {
        cube([THICKNESS, HEIGHT - OFFSET, DEPTH]);
    }

    rotate([90, 0, 0]) {
        /* Bottom face */
        translate([0, 0, -HEIGHT + COVER_HEIGHT]) {
            cube([SERVO_AXLE_LENGTH / 2, DEPTH, THICKNESS]);
        }

        /* Bottom support columnns */
        translate([SERVO_AXLE_LENGTH / 2 - 12, 0, -HEIGHT + COVER_HEIGHT - 4]) {
            cube([10, DEPTH, 18]);
        }


        /* Battery compartment separator */
        translate([0, 0, -HEIGHT + OFFSET]) {
            difference() {
                cube([COVER_WIDTH / 2, DEPTH, THICKNESS]);
                translate([0, 0, -T]) {
                    cube([SERVO_AXLE_LENGTH / 2, 20, THICKNESS + T * 2]);
                    translate([0, 20, 0]) {
                        /* Arc to ease 3D printing */
                        cylinder(h = THICKNESS * 2, d = SERVO_AXLE_LENGTH - THICKNESS * 2, $fn = R);
                        translate([0, 80, 0])
                        cylinder(h = THICKNESS * 2, d = SERVO_AXLE_LENGTH - THICKNESS * 2, $fn = R);
                    }
                }
            }
        }
    }

    /* Top face */
    translate([0, HEIGHT - THICKNESS, 0]) {
        cube([WIDTH / 2, THICKNESS, DEPTH]);

    }
}


module half() {
    difference() {
        union() {
            face();
            difference() {
                rotate([TILT, 0, 0]) {
                    frame();
                }

                /* Line following sensor wires */
                translate([0, 32, THICKNESS]) {
                    rotate([0, -90, 0]) {
                        cylinder(d = 26, h = 30, $fn = R);
                    }
                    rotate([90, 0, 0]) {
                        cylinder(d = 26, h = 100, $fn = R);
                    }

                }

                rotate([TILT]) {
                    rotate([0, -90, 0]) {
                        /* Bottom arc */
                        translate([50, -86, -50]) {
                            cylinder(d = 195, h = 100, $fn = R);
                        }

                        /* Servo mount */
                        translate([SERVO_MOUNT_PLACEMENT, SERVO_MOUNT_OFFSET, 0]) {
                            servo_cutout();
                        }
                    }
                }

            }
        }

        /* Front face cutoff */
        translate([-100, -100, -100]) {
            cube([200, 300, 100]);
        }

        /* Bottom face cutoff */
        translate([-100, -100, -100]) {
            cube([300, 100, 300]);
        }

        /* Top face cutoff */
        rotate([TILT, 0, 0]) {
            translate([-100, HEIGHT, -100]) {
                cube([200, 200, 200]);
            }

            translate([-COVER_WIDTH / 2 - T, HEIGHT - COVER_HEIGHT - THICKNESS, DEPTH - THICKNESS]) {
                cube([COVER_WIDTH + T, 200, 200]);
            }


        }
    }
}

module magnet_hole() {
    cylinder(d = MAGNET_DIAMETER + T, h = MAGNET_HEIGHT + T, $fn = R);
}

module magnet_holes() {
    mirror_copy() {
        /* Top holes */
        translate([+COVER_WIDTH / 2 - MAGNET_DIAMETER / 2 - 0.5, HEIGHT - 3, -MAGNET_HEIGHT]) {
            magnet_hole();
        }
        /* Bottom holes */
        translate([SERVO_AXLE_LENGTH / 2 - MAGNET_DIAMETER / 2 - 0.5, HEIGHT - COVER_HEIGHT + MAGNET_DIAMETER / 2 - THICKNESS + 0.5, -MAGNET_HEIGHT]) {
            magnet_hole();
        }
    }
}

module sumorobot() {
    difference() {
        mirror_copy() {
            half();
        }
        rotate([TILT, 0, 0]) {
            translate([0, 0, DEPTH - THICKNESS]) {
                magnet_holes();
            }
        }
    }
}


module cover() {
    translate([0, 100, 0]) {
        difference() {
          union() {
            translate([-27, COVER_HEIGHT-20, 0])
            cube([35,12,THICKNESS*2]);

            mirror_copy() {
                // Battery/servo compartment cover
                translate([0, 0, 0]) {
                    cube([SERVO_AXLE_LENGTH / 2, COVER_HEIGHT, THICKNESS]);
                }

                // PCB compartment cover
                translate([0, COVER_HEIGHT - OFFSET, 0]) {
                    cube([COVER_WIDTH / 2, OFFSET + THICKNESS, THICKNESS]);
                }

                // PCB locking mechanism
                translate([PCB_WIDTH/2-T-6, COVER_HEIGHT-6, 0]) {
                    cube([6, PCB_THICKNESS-T, THICKNESS+3*PCB_THICKNESS]);

                    cube([PCB_THICKNESS, 5, THICKNESS+3*PCB_THICKNESS]);
                }
            }
          }
            translate([0, COVER_HEIGHT - HEIGHT + THICKNESS, THICKNESS]) {
                magnet_holes();
            }
            translate([-COVER_WIDTH/2+THICKNESS-0.5, COVER_HEIGHT+THICKNESS-0.5-PCB_THICKNESS, -T]) {
                translate([10.7, -13.1, 0]) {
                   cylinder(d2=5, d1=5+THICKNESS, h=THICKNESS+T*2, $fn=R);
                   cylinder(d=5, h=10, $fn=R);

                }
                translate([36.2, -13.1, 0]) cylinder(d=5, h=THICKNESS*3+T*2, $fn=R);
                translate([17.8-T*2, -19.46, 0]) cube([12.1+T*4, 10.5+T*4, 20]);
            }



        }
    }
}

module plow_hole() {
    translate([0, 0, -T])
    cylinder(d1=LINE_SENSOR_HOLES+T, d2=LINE_SENSOR_HOLES*3, h=THICKNESS+T+T, $fn=R);
    translate([0, 0, THICKNESS]) {
        cylinder(d=LINE_SENSOR_HOLES*3, h=WIDTH, $fn=R);
    }
}

module plow() {
    translate([0, -40, 0]) {
        mirror_copy() {
            difference() {
                rotate([TILT, 0, 0]) {
                    cube([WIDTH/2, LINE_SENSOR_PLACEMENT*2-6, 50]);
                }

                rotate([-TILT, 0, 0])
                translate([-T, LINE_SENSOR_PLACEMENT*2-6, -6])
                cube([100, 100, 100]);
                translate([-T, 0, -100]) {
                    cube([100, 100, 100]);
                }

                translate([0, LINE_SENSOR_PLACEMENT-3]) {
                    translate([LINE_SENSOR_SPAN, 0]) {
                        plow_hole();
                    }
                    plow_hole();
                }
                translate([0, -1, 50]) {
                    rotate([-90, 0, 0]) {
                        cylinder(d=WIDTH-10, h=50, $fn=R);
                    }
                }
            }
        }
    }
}

sumorobot();
cover();
plow();

if (DEBUG) {
    color([1, 0, 0]) {
        rotate([TILT, 0, 0]) {
            rotate([90, 0, 0]) {
                /* PCB board mockup */
                translate([0, 0, -HEIGHT + PCB_OFFSET + T]) {
                    translate([-PCB_WIDTH / 2, DEPTH - PCB_DEPTH, 0]) {
                        cube([PCB_WIDTH, PCB_DEPTH + 5, 1.6]);
                    }

                    translate([-PCB_WIDTH / 2 + 5, DEPTH - PCB_DEPTH, 0]) {
                        cube([PCB_WIDTH - 10, PCB_DEPTH - 5, 20]);
                    }
                }

                /* Battery mockup */
                translate([-25, 30, -24]) {
                    translate([0, 0, 0]) {
                        cube([50, 60, 5.5]);
                    }
                    translate([10, 0, 0]) {
                        cube([30, 50, 10]);
                    }
                }
            }
        }

        translate([WIDTH / 2 + 28 / 2, HEIGHT - 47, 0]) {
            cylinder(h = 12, d = 15, $fn = R);
        }

        /* Wheel mockups */
        rotate([TILT]) {
            rotate([0, -90, 0]) {
                translate([SERVO_MOUNT_PLACEMENT, SERVO_MOUNT_OFFSET, SERVO_AXLE_LENGTH/2 + 10]) {
                    difference() {
                        cylinder(d=70, h=11);
                        cylinder(d=3, h=12);
                    }
                }
            }
        }

        /* Line sensor mockups */
        difference() {
            translate([0, LINE_SENSOR_PLACEMENT - 5.2, THICKNESS]) {
                translate([WIDTH / 2 - (+12 + 8.9), 0, 0]) cube([17.8, 15.2, 5.2]);
                translate([-8.9, 0, 0]) cube([17.8, 15.2, 5.2]);
                translate([-WIDTH / 2 + 12 - 8.9, 0, 0]) cube([17.8, 15.2, 5.2]);
            }
            cylinder(d = 3, h = 20, $fn = R);
            translate([38, 0, 0]) cylinder(d = 3, h = 20, $fn = R);
        }
    }
}